Repository: uzh-rpg/deep_drone_acrobatics Branch: master Commit: c533a6ea9f35 Files: 174 Total size: 608.2 KB Directory structure: gitextract_nr_44o_q/ ├── .clang-format ├── .gitignore ├── LICENSE ├── README.md ├── acrobatic_trajectory_helper/ │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include/ │ │ └── acrobatic_trajectory_helper/ │ │ ├── acrobatic_sequence.h │ │ ├── circle_trajectory_helper.h │ │ ├── heading_trajectory_helper.h │ │ └── polynomial_trajectory_helper.h │ ├── package.xml │ └── src/ │ ├── acrobatic_sequence.cpp │ ├── circle_trajectory_helper.cpp │ ├── heading_trajectory_helper.cpp │ └── polynomial_trajectory_helper.cpp ├── conda_requirements.txt ├── controller_learning/ │ ├── CMakeLists.txt │ ├── README.md │ ├── ckpt/ │ │ └── loop/ │ │ ├── fts_and_imu/ │ │ │ ├── ckpt-7.data-00000-of-00002 │ │ │ ├── ckpt-7.data-00001-of-00002 │ │ │ └── ckpt-7.index │ │ └── imu_only/ │ │ ├── ckpt-37.data-00000-of-00002 │ │ ├── ckpt-37.data-00001-of-00002 │ │ └── ckpt-37.index │ ├── common.py │ ├── config/ │ │ ├── dagger_settings.yaml │ │ ├── settings.py │ │ ├── test_settings.yaml │ │ └── train_settings.yaml │ ├── data/ │ │ └── .gitignore │ ├── iterative_learning_trajectories.py │ ├── launch/ │ │ └── simulation/ │ │ ├── base_quad_simulation_no_controller.launch │ │ ├── controller.launch │ │ ├── custom_rotors_interface.launch │ │ ├── minisim.launch │ │ ├── quadrotor_empty_world_no_controller.launch │ │ ├── quadrotor_rgb_world_no_controller.launch │ │ ├── rpg_mpc.launch │ │ ├── rpg_pid.launch │ │ ├── rpg_rotors_interface.launch │ │ ├── rviz_only.launch │ │ └── simulation.launch │ ├── package.xml │ ├── results/ │ │ └── .gitignore │ ├── setup.py │ ├── src/ │ │ └── ControllerLearning/ │ │ ├── TrajectoryBase.py │ │ ├── TrajectoryLearning.py │ │ ├── __init__.py │ │ └── models/ │ │ ├── __init__.py │ │ ├── body_dataset.py │ │ ├── bodyrate_learner.py │ │ ├── nets.py │ │ └── tf_addons_normalizations.py │ ├── test_trajectories.py │ ├── train.py │ └── train_logs/ │ ├── 20191015-164750/ │ │ └── train/ │ │ ├── checkpoint │ │ ├── ckpt-78.data-00000-of-00002 │ │ ├── ckpt-78.data-00001-of-00002 │ │ ├── ckpt-78.index │ │ ├── ckpt-79.data-00000-of-00002 │ │ ├── ckpt-79.data-00001-of-00002 │ │ ├── ckpt-79.index │ │ ├── ckpt-80.data-00000-of-00002 │ │ ├── ckpt-80.data-00001-of-00002 │ │ ├── ckpt-80.index │ │ └── events.out.tfevents.1571150870.elia-ThinkPad-P51.9892.82.v2 │ ├── 20191022-082718/ │ │ └── train/ │ │ ├── checkpoint │ │ ├── ckpt-1078.data-00000-of-00002 │ │ ├── ckpt-1078.data-00001-of-00002 │ │ ├── ckpt-1078.index │ │ ├── ckpt-1079.data-00000-of-00002 │ │ ├── ckpt-1079.data-00001-of-00002 │ │ ├── ckpt-1079.index │ │ ├── ckpt-1080.data-00000-of-00002 │ │ ├── ckpt-1080.data-00001-of-00002 │ │ ├── ckpt-1080.index │ │ └── events.out.tfevents.1571725638.elia-ThinkPad-P51.29578.82.v2 │ ├── 20191022-095948/ │ │ └── train/ │ │ ├── checkpoint │ │ ├── ckpt-1000.data-00000-of-00002 │ │ ├── ckpt-1000.data-00001-of-00002 │ │ ├── ckpt-1000.index │ │ ├── ckpt-998.data-00000-of-00002 │ │ ├── ckpt-998.data-00001-of-00002 │ │ ├── ckpt-998.index │ │ ├── ckpt-999.data-00000-of-00002 │ │ ├── ckpt-999.data-00001-of-00002 │ │ ├── ckpt-999.index │ │ └── events.out.tfevents.1571731188.elia-ThinkPad-P51.5931.82.v2 │ ├── 20191022-122145/ │ │ └── train/ │ │ └── events.out.tfevents.1571739705.elia-ThinkPad-P51.15206.54.v2 │ ├── 20191022-122316/ │ │ └── train/ │ │ └── events.out.tfevents.1571739796.elia-ThinkPad-P51.16327.54.v2 │ ├── 20191022-122551/ │ │ └── train/ │ │ └── events.out.tfevents.1571739951.elia-ThinkPad-P51.16978.54.v2 │ ├── 20191022-122626/ │ │ └── train/ │ │ └── events.out.tfevents.1571739986.elia-ThinkPad-P51.17446.54.v2 │ ├── 20191022-122842/ │ │ └── train/ │ │ └── events.out.tfevents.1571740122.elia-ThinkPad-P51.17988.54.v2 │ ├── 20191022-123011/ │ │ └── train/ │ │ └── events.out.tfevents.1571740211.elia-ThinkPad-P51.18467.54.v2 │ ├── 20191022-134158/ │ │ └── train/ │ │ └── events.out.tfevents.1571744518.elia-ThinkPad-P51.27889.54.v2 │ ├── 20191022-135227/ │ │ └── train/ │ │ └── events.out.tfevents.1571745147.elia-ThinkPad-P51.31014.54.v2 │ ├── 20191022-135514/ │ │ └── train/ │ │ └── events.out.tfevents.1571745314.elia-ThinkPad-P51.32244.54.v2 │ ├── 20191023-115641/ │ │ └── train/ │ │ └── events.out.tfevents.1571824601.elia-ThinkPad-P51.12466.54.v2 │ ├── 20191023-115844/ │ │ └── train/ │ │ └── events.out.tfevents.1571824724.elia-ThinkPad-P51.13447.54.v2 │ ├── 20191023-120327/ │ │ └── train/ │ │ └── events.out.tfevents.1571825007.elia-ThinkPad-P51.15260.54.v2 │ ├── 20191023-120523/ │ │ └── train/ │ │ └── events.out.tfevents.1571825123.elia-ThinkPad-P51.16206.54.v2 │ ├── 20191023-120641/ │ │ └── train/ │ │ └── events.out.tfevents.1571825201.elia-ThinkPad-P51.17007.54.v2 │ ├── 20191023-121605/ │ │ └── train/ │ │ └── events.out.tfevents.1571825765.elia-ThinkPad-P51.19388.54.v2 │ ├── 20191023-122039/ │ │ └── train/ │ │ └── events.out.tfevents.1571826039.elia-ThinkPad-P51.21241.54.v2 │ └── 20191023-122438/ │ └── train/ │ └── events.out.tfevents.1571826278.elia-ThinkPad-P51.23680.54.v2 ├── custom_rotors_interface/ │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include/ │ │ └── custom_rotors_interface/ │ │ └── custom_rotors_interface.h │ ├── package.xml │ ├── parameters/ │ │ ├── autopilot.yaml │ │ ├── custom_rotors_interface.yaml │ │ ├── manual_flight_assistant.yaml │ │ └── position_controller.yaml │ └── src/ │ ├── custom_rotors_interface.cpp │ └── custom_rotors_interface_node.cpp ├── dependencies.yaml ├── fpv_aggressive_trajectories/ │ ├── .gitignore │ ├── CMakeLists.txt │ ├── include/ │ │ └── fpv_aggressive_trajectories/ │ │ ├── fpv_aggressive_trajectories.h │ │ ├── odometry_republisher.h │ │ └── visualize.h │ ├── launch/ │ │ └── simulation/ │ │ ├── controller.launch │ │ ├── custom_rotors_interface.launch │ │ ├── rpg_mpc.launch │ │ ├── rpg_pid.launch │ │ └── simulation.launch │ ├── package.xml │ ├── parameters/ │ │ └── mpc_params.yaml │ ├── resources/ │ │ ├── race_track/ │ │ │ ├── iros_materials/ │ │ │ │ └── materials/ │ │ │ │ └── scripts/ │ │ │ │ ├── asphalt.material │ │ │ │ ├── carpet.material │ │ │ │ ├── wall_1.material │ │ │ │ ├── wall_2.material │ │ │ │ ├── wall_3.material │ │ │ │ └── wall_4.material │ │ │ └── real_world/ │ │ │ └── gate/ │ │ │ └── meshes/ │ │ │ └── gate.dae │ │ ├── rviz/ │ │ │ ├── race_track.rviz │ │ │ └── real_world.rviz │ │ ├── simulation/ │ │ │ ├── urdf/ │ │ │ │ ├── component_snippets.xacro │ │ │ │ ├── hummingbird.xacro │ │ │ │ ├── hummingbird_base.xacro │ │ │ │ └── mav_generic_odometry_sensor.gazebo │ │ │ └── vehicles/ │ │ │ ├── hummingbird.gazebo │ │ │ └── hummingbird_rgbcamera300200.gazebo │ │ ├── urdf/ │ │ │ ├── component_snippets.xacro │ │ │ ├── hummingbird_base.xacro │ │ │ └── mav_generic_odometry_sensor_rgb.gazebo │ │ └── worlds/ │ │ ├── controller_learning.world │ │ ├── random_gates.world │ │ ├── single_gate.world │ │ └── track.world │ ├── rviz/ │ │ └── trajectory_comparison_looping_sim.rviz │ └── src/ │ ├── fpv_aggressive_trajectories.cpp │ ├── odometry_republisher.cpp │ └── visualize.cpp ├── odometry_converter/ │ ├── CMakeLists.txt │ ├── include/ │ │ └── odometry_converter/ │ │ └── odometry_converter.h │ ├── package.xml │ └── src/ │ ├── odometry_converter.cpp │ └── odometry_converter_node.cpp ├── pip_requirements.txt ├── test_performance/ │ ├── CMakeLists.txt │ ├── README.md │ ├── __init__.py │ ├── launch/ │ │ └── test_performance.launch │ ├── nodes/ │ │ └── test_performance.py │ ├── package.xml │ ├── setup.py │ └── src/ │ └── TestPerformance/ │ ├── TestPerformance.py │ └── __init__.py └── trajectory_visualizer/ ├── .gitignore ├── CMakeLists.txt ├── include/ │ └── trajectory_visualizer/ │ └── trajectory_visualizer.h ├── package.xml └── src/ └── trajectory_visualizer.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ --- Language: Cpp # BasedOnStyle: Google AccessModifierOffset: -1 AlignAfterOpenBracket: Align AlignConsecutiveAssignments: false AlignConsecutiveDeclarations: false AlignEscapedNewlines: Left AlignOperands: true AlignTrailingComments: true AllowAllParametersOfDeclarationOnNextLine: true AllowShortBlocksOnASingleLine: false AllowShortCaseLabelsOnASingleLine: false AllowShortFunctionsOnASingleLine: All AllowShortIfStatementsOnASingleLine: true AllowShortLoopsOnASingleLine: true AlwaysBreakAfterDefinitionReturnType: None AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: true AlwaysBreakTemplateDeclarations: true BinPackArguments: true BinPackParameters: true BraceWrapping: AfterClass: false AfterControlStatement: false AfterEnum: false AfterFunction: false AfterNamespace: false AfterObjCDeclaration: false AfterStruct: false AfterUnion: false AfterExternBlock: false BeforeCatch: false BeforeElse: false IndentBraces: false SplitEmptyFunction: true SplitEmptyRecord: true SplitEmptyNamespace: true BreakBeforeBinaryOperators: None BreakBeforeBraces: Attach BreakBeforeInheritanceComma: false BreakBeforeTernaryOperators: true BreakConstructorInitializersBeforeComma: false BreakConstructorInitializers: BeforeColon BreakAfterJavaFieldAnnotations: false BreakStringLiterals: true ColumnLimit: 80 CommentPragmas: '^ IWYU pragma:' CompactNamespaces: false ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true DerivePointerAlignment: true DisableFormat: false ExperimentalAutoDetectBinPacking: false FixNamespaceComments: true ForEachMacros: - foreach - Q_FOREACH - BOOST_FOREACH IncludeBlocks: Preserve IncludeCategories: - Regex: '^' Priority: 2 - Regex: '^<.*\.h>' Priority: 1 - Regex: '^<.*' Priority: 2 - Regex: '.*' Priority: 3 IncludeIsMainRegex: '([-_](test|unittest))?$' IndentCaseLabels: true IndentPPDirectives: None IndentWidth: 2 IndentWrappedFunctionNames: false JavaScriptQuotes: Leave JavaScriptWrapImports: true KeepEmptyLinesAtTheStartOfBlocks: false MacroBlockBegin: '' MacroBlockEnd: '' MaxEmptyLinesToKeep: 1 NamespaceIndentation: None ObjCBlockIndentWidth: 2 ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: false PenaltyBreakAssignment: 2 PenaltyBreakBeforeFirstCallParameter: 1 PenaltyBreakComment: 300 PenaltyBreakFirstLessLess: 120 PenaltyBreakString: 1000 PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 200 PointerAlignment: Left #RawStringFormats: # - Delimiter: pb # Language: TextProto # BasedOnStyle: google ReflowComments: true SortIncludes: true SortUsingDeclarations: true SpaceAfterCStyleCast: false SpaceAfterTemplateKeyword: true SpaceBeforeAssignmentOperators: true SpaceBeforeParens: ControlStatements SpaceInEmptyParentheses: false SpacesBeforeTrailingComments: 2 SpacesInAngles: false SpacesInContainerLiterals: true SpacesInCStyleCastParentheses: false SpacesInParentheses: false SpacesInSquareBrackets: false Standard: Auto TabWidth: 8 UseTab: Never ... ================================================ FILE: .gitignore ================================================ .idea/ cmake-build-debug/ __pycache__/ *.pyc ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2020 Robotics and Perception Group Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ # Deep Drone Acrobatics This repo contains the code associated to our paper Deep Drone Acrobatics.

dda

#### Citing If you use this code in an academic context, please cite the following publication: Paper: [Deep Drone Acrobatics](http://rpg.ifi.uzh.ch/docs/RSS20_Kaufmann.pdf) RSS 2020 Video Pitch: [Youtube](https://youtu.be/r4zzdFw87CY) Video: [YouTube](https://youtu.be/2N_wKXQ6MXA) ``` @inproceedings{kaufmann2020RSS, title={Deep Drone Acrobatics}, author={Kaufmann, Elia and Loquercio, Antonio and Ranftl, Ren{\'e} and M{\"u}ller, Matthias and Koltun, Vladlen and Scaramuzza, Davide}, booktitle={Proceedings of Robotics: Science and Systems}, year={2020}, address={Corvalis, Oregon, USA}, month={July}, doi={10.15607/RSS.2020.XVI.040} } ``` ## Installation ### Requirements The code was tested with Ubuntu 18.04, ROS Melodic, Anaconda v4.8.3. Different OS and ROS versions are possible but not supported. ### Step-by-Step Procedure Use the following commands to create a new catkin workspace and a virtual environment with all the required dependencies. ```bash export ROS_VERSION=melodic mkdir drone_acrobatics_ws cd drone_acrobatics_ws export CATKIN_WS=./catkin_dda mkdir -p $CATKIN_WS/src cd $CATKIN_WS catkin init catkin config --extend /opt/ros/$ROS_VERSION catkin config --merge-devel catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-fdiagnostics-color cd src git clone https://github.com/uzh-rpg/deep_drone_acrobatics.git vcs-import < deep_drone_acrobatics/dependencies.yaml #install extra dependencies (might need more depending on your OS) sudo apt-get install libqglviewer-dev-qt5 ``` Before continuing, make sure that your protobuf compiler version is 3.0.0. To check this out, type in a terminal ``protoc --version``. If This is not the case, then check out [this guide](https://github.com/linux-on-ibm-z/docs/wiki/Building-ProtoBuf-3.0.0) on how to do it. Note that building the VINS-Mono package requires to install the [Ceres Solver](http://ceres-solver.org/index.html). You can find installation instructions [here](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) or [here](http://ceres-solver.org/installation.html). ```bash # Build and re-source the workspace catkin build . ../devel/setup.bash # Create your learning environment cd deep_drone_acrobatics conda create --name drone_flow python=3.6 conda activate drone_flow # Install (in an hacky way) python requirements pip install -r pip_requirements.txt conda install --file conda_requirements.txt ``` ## Let's do a Power Loop Once you have installed the dependencies, you will be able to fly in simulation with our pre-trained checkpoint. You don't need necessarely need a GPU for execution. Note that if the network can't run at least at 15Hz, you won't be able to fly successfully. Lauch a simulation! Open a terminal and type: ```bash cd drone_acrobatics_ws source catkin_dda/devel/setup.bash roslaunch fpv_aggressive_trajectories simulation.launch ``` Run the Network in an other terminal: ```bash cd cd drone_acrobatics_ws . ./catkin_dda/devel/setup.bash conda activate drone_flow python test_trajectories.py --settings_file=config/test_settings.yaml ``` ## Train your own acrobatic controller You can use the following commands to generate data in simulation and train your model on it. The trained checkpoint can then be used to control a physical platform (if you have one!). ### Generate data Launch the simulation in one terminal ```bash cd drone_acrobatics_ws source catkin_dda/devel/setup.bash roslaunch fpv_aggressive_trajectories simulation.launch ``` Launch data collection (with dagger) in an other terminal ```bash cd cd drone_acrobatics_ws . ./catkin_dda/devel/setup.bash conda activate drone_flow python iterative_learning_trajectories.py --settings_file=config/dagger_settings.yaml ``` It is possible to change parameters (number of rollouts, history length, etc. ) in the file [dagger\_settings.yaml](./controller_learning/config/dagger_settings.yaml). Keep in mind that if you change the network (for example by changing the history length), you will need to adapt the file [test_settings.yaml](./controller_learning/config/test_settings.yaml) for compatibility. ### Train the Network If you want to train the network on data already collected (for example to do some parameter tuning) use the following commands. Make sure to adapt the settings file to your configuration. ```bash cd cd drone_acrobatics_ws . ./catkin_dda/devel/setup.bash conda activate drone_flow python train.py --settings_file=config/train_settings.yaml ``` ### Test the Network To test the network you trained, adapt the [test_settings.yaml](./controller_learning/config/test_settings.yaml) with the new checkpoint path. Then follow the instruction to test! Lauch a simulation! Open a terminal and type: ```bash cd drone_acrobatics_ws source catkin_dda/devel/setup.bash roslaunch fpv_aggressive_trajectories simulation.launch ``` Run the Network in an other terminal: ```bash cd cd drone_acrobatics_ws . ./catkin_dda/devel/setup.bash conda activate drone_flow python test_trajectories.py --settings_file=config/test_settings.yaml ``` ================================================ FILE: acrobatic_trajectory_helper/.gitignore ================================================ .idea/ cmake-build-debug/ ================================================ FILE: acrobatic_trajectory_helper/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(acrobatic_trajectory_helper) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) add_compile_options(-O3) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) cs_add_library(${PROJECT_NAME} src/polynomial_trajectory_helper.cpp src/heading_trajectory_helper.cpp src/circle_trajectory_helper.cpp) cs_add_library(acrobatic_sequence src/acrobatic_sequence) target_link_libraries(acrobatic_sequence ${PROJECT_NAME}) cs_install() cs_export() ================================================ FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/acrobatic_sequence.h ================================================ #pragma once #include #include namespace fpv_aggressive_trajectories { class AcrobaticSequence { public: explicit AcrobaticSequence( const quadrotor_common::TrajectoryPoint& start_state); virtual ~AcrobaticSequence(); bool appendLoops(const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end, const double& traj_sampling_freq); bool appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end); bool appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end); std::list getManeuverList(); private: std::list maneuver_list_; }; } // namespace fpv_aggressive_trajectories ================================================ FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/circle_trajectory_helper.h ================================================ #pragma once #include #include namespace acrobatic_trajectory_helper { namespace circles { quadrotor_common::Trajectory computeHorizontalCircleTrajectory( const Eigen::Vector3d center, const double radius, const double speed, const double phi_start, const double phi_end, const double sampling_frequency); quadrotor_common::Trajectory computeVerticalCircleTrajectory( const Eigen::Vector3d center, const double orientation, const double radius, const double speed, const double phi_start, const double phi_end, const double sampling_frequency); quadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew( const Eigen::Vector3d center, const double orientation, const double radius, const double speed, const double phi_start, const double phi_end, const double corkscrew_velocity, const double sampling_frequency); } // namespace circles } // namespace acrobatic_trajectory_helper ================================================ FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/heading_trajectory_helper.h ================================================ #pragma once #include namespace acrobatic_trajectory_helper { namespace heading { void addConstantHeading(const double heading, quadrotor_common::Trajectory* trajectory); void addConstantHeadingRate(const double initial_heading, const double final_heading, quadrotor_common::Trajectory* trajectory); } // namespace heading } // namespace acrobatic_trajectory_helper ================================================ FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/polynomial_trajectory_helper.h ================================================ #pragma once #include #include #include #include #include namespace acrobatic_trajectory_helper { namespace polynomials { // Constrained Polynomials quadrotor_common::Trajectory computeTimeOptimalTrajectory( const quadrotor_common::TrajectoryPoint& s0, const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency); quadrotor_common::Trajectory computeFixedTimeTrajectory( const quadrotor_common::TrajectoryPoint& s0, const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, const double execution_time, const double sampling_frequency); // Minimum Snap Style Polynomials quadrotor_common::Trajectory generateMinimumSnapTrajectory( const Eigen::VectorXd& segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapTrajectory( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( const Eigen::VectorXd& segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency); quadrotor_common::Trajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency); // Sampling function quadrotor_common::Trajectory samplePolynomial( const polynomial_trajectories::PolynomialTrajectory& polynomial, const double sampling_frequency); } // namespace polynomials } // namespace acrobatic_trajectory_helper ================================================ FILE: acrobatic_trajectory_helper/package.xml ================================================ acrobatic_trajectory_helper 0.0.0 The acrobatic_trajectory_helper package Elia Kaufmann MIT Elia Kaufmann catkin catkin_simple eigen_catkin minimum_jerk_trajectories polynomial_trajectories quadrotor_common roscpp ================================================ FILE: acrobatic_trajectory_helper/src/acrobatic_sequence.cpp ================================================ #include "acrobatic_trajectory_helper/acrobatic_sequence.h" #include #include #include #include #include #include #include #include #include namespace fpv_aggressive_trajectories { AcrobaticSequence::AcrobaticSequence( const quadrotor_common::TrajectoryPoint& start_state) { printf("Initiated acrobatic sequence\n"); quadrotor_common::Trajectory init_trajectory; quadrotor_common::TrajectoryPoint init_point; init_point = start_state; init_trajectory.points.push_back(init_point); maneuver_list_.push_back(init_trajectory); } AcrobaticSequence::~AcrobaticSequence() {} bool AcrobaticSequence::appendLoops( const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end, const double& traj_sampling_freq) { printf("appending loop\n"); // get start state quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back(); printf( "Enter state of loop maneuver: Pos: %.2f, %.2f, %.2f | Vel: %.2f, %.2f, " "%.2f\n", init_state.position.x(), init_state.position.y(), init_state.position.z(), init_state.velocity.x(), init_state.velocity.y(), init_state.velocity.z()); const double exec_loop_rate = traj_sampling_freq; const double desired_heading = 0.0; const double figure_z_rotation_angle = 0.0; // const double figure_z_rotation_angle = 0.785398163; const Eigen::Quaterniond q_W_P = Eigen::Quaterniond( Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ())); double desired_heading_loop = quadrotor_common::wrapMinusPiToPi( desired_heading + figure_z_rotation_angle); // cirlce center RELATIVE to start position const Eigen::Vector3d circle_center = init_state.position + q_W_P.inverse() * circle_center_offset; printf("circle center: %.2f, %.2f, %.2f\n", circle_center.x(), circle_center.y(), circle_center.z()); const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius; const double max_roll_pitch_rate = 3.0; // Compute Circle trajectory printf("compute circle trajectory\n"); quadrotor_common::Trajectory circle_trajectory = acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory( circle_center, figure_z_rotation_angle, radius, circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( desired_heading_loop, &circle_trajectory); quadrotor_common::TrajectoryPoint circle_enter_state = circle_trajectory.points.front(); // Start position relative to circle center quadrotor_common::TrajectoryPoint start_state; start_state = init_state; // enter trajectory printf("compute enter trajectory\n"); // printf("Maximum speed: %.3f, current speed: %.3f\n", 1.1*circle_velocity, // start_state.velocity.norm()); quadrotor_common::Trajectory enter_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( start_state, circle_enter_state, 4, 1.1 * std::max(start_state.velocity.norm(), circle_velocity), max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( desired_heading_loop, &enter_trajectory); // End position RELATIVE to circle center printf("compute exit trajectory\n"); const Eigen::Vector3d end_pos_P = circle_center_offset_end; // Eigen::Vector3d(circle_center_offset.x(), // scircle_center_offset.y(),-circle_center_offset.z()); // nice breaking // forward flip quadrotor_common::TrajectoryPoint end_state; end_state.position = q_W_P * end_pos_P + circle_center; end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0); quadrotor_common::Trajectory exit_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( desired_heading_loop, &exit_trajectory); quadrotor_common::Trajectory breaking_trajectory; breaking_trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; maneuver_list_.push_back(enter_trajectory); maneuver_list_.push_back(circle_trajectory); maneuver_list_.push_back(exit_trajectory); if (break_at_end) { // append breaking trajectory at end quadrotor_common::TrajectoryPoint end_state_hover; end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0)); end_state_hover.velocity = Eigen::Vector3d::Zero(); breaking_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0, max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( 0.0, &breaking_trajectory); maneuver_list_.push_back(breaking_trajectory); } return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED); } bool AcrobaticSequence::appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end) { printf("appending matty loop\n"); // get start state quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back(); const double exec_loop_rate = 50.0; const double desired_heading = M_PI; const double figure_z_rotation_angle = 0.0; const Eigen::Quaterniond q_W_P = Eigen::Quaterniond( Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ())); double desired_heading_loop = quadrotor_common::wrapMinusPiToPi( desired_heading + figure_z_rotation_angle); // cirlce center RELATIVE to start position const Eigen::Vector3d circle_center = init_state.position + q_W_P.inverse() * circle_center_offset; const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius; const double max_roll_pitch_rate = 3.0; quadrotor_common::Trajectory circle_trajectory = acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory( circle_center, figure_z_rotation_angle, radius, circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( desired_heading_loop, &circle_trajectory); quadrotor_common::TrajectoryPoint circle_enter_state = circle_trajectory.points.front(); // Start position relative to circle center quadrotor_common::TrajectoryPoint start_state; start_state = init_state; // enter trajectory quadrotor_common::Trajectory enter_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( start_state, circle_enter_state, 4, 1.1 * std::max(start_state.velocity.norm(), circle_velocity), max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeadingRate(0.0, M_PI, &enter_trajectory); const Eigen::Vector3d end_pos_P = circle_center_offset_end; quadrotor_common::TrajectoryPoint end_state; end_state.position = q_W_P * end_pos_P + circle_center; end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0); quadrotor_common::Trajectory exit_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeadingRate(M_PI, 2 * M_PI, &exit_trajectory); // append breaking trajectory at end quadrotor_common::TrajectoryPoint end_state_hover; end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0)); end_state_hover.velocity = Eigen::Vector3d::Zero(); quadrotor_common::Trajectory breaking_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0, max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( 0.0, &breaking_trajectory); maneuver_list_.push_back(enter_trajectory); maneuver_list_.push_back(circle_trajectory); maneuver_list_.push_back(exit_trajectory); maneuver_list_.push_back(breaking_trajectory); return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED); } bool AcrobaticSequence::appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius, const Eigen::Vector3d& circle_center_offset, const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end) { printf("appending barell roll\n"); // get start state quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back(); const double exec_loop_rate = 50.0; const double desired_heading = 0.0; const double circle_radius = radius; double corkscrew_velocity = 0.5; const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / circle_radius; const double max_roll_pitch_rate = 3.0; const double figure_z_rotation_angle = M_PI / 2.0; double desired_heading_loop = quadrotor_common::wrapMinusPiToPi( desired_heading + figure_z_rotation_angle); quadrotor_common::TrajectoryPoint circle_enter_state; circle_enter_state.position = init_state.position + circle_center_offset; circle_enter_state.velocity = Eigen::Vector3d(corkscrew_velocity, circle_velocity, 0.0); // compute enter trajectory quadrotor_common::Trajectory enter_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( init_state, circle_enter_state, 4, 1.5 * std::max(init_state.velocity.norm(), circle_velocity), max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading(0.0, &enter_trajectory); // cirlce center RELATIVE to start position const Eigen::Vector3d circle_center = circle_enter_state.position + Eigen::Vector3d(0.0, 0.0, circle_radius); // Compute Circle trajectory double rotate_loop = M_PI / 2.0; quadrotor_common::Trajectory circle_trajectory = acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectoryCorkScrew( circle_center, rotate_loop, circle_radius, circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, corkscrew_velocity, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( 0, &circle_trajectory); const Eigen::Quaterniond q_W_P = Eigen::Quaterniond( Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ())); quadrotor_common::TrajectoryPoint end_state; end_state.position = circle_trajectory.points.back().position + circle_center_offset_end; end_state.velocity = Eigen::Vector3d(circle_velocity, 0.0, 0.0); quadrotor_common::TrajectoryPoint circle_exit_state = circle_trajectory.points.back(); quadrotor_common::Trajectory exit_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( circle_exit_state, end_state, 4, 1.5 * circle_velocity, max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading(0.0, &exit_trajectory); maneuver_list_.push_back(enter_trajectory); maneuver_list_.push_back(circle_trajectory); maneuver_list_.push_back(exit_trajectory); quadrotor_common::Trajectory breaking_trajectory; breaking_trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; if (break_at_end) { // append breaking trajectory at end quadrotor_common::TrajectoryPoint end_state_hover; end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0)); end_state_hover.velocity = Eigen::Vector3d::Zero(); breaking_trajectory = acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory( end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0, max_roll_pitch_rate, exec_loop_rate); acrobatic_trajectory_helper::heading::addConstantHeading( 0.0, &breaking_trajectory); maneuver_list_.push_back(breaking_trajectory); } // Debug output std::cout << static_cast(enter_trajectory.trajectory_type) << std::endl; std::cout << static_cast(circle_trajectory.trajectory_type) << std::endl; std::cout << static_cast(exit_trajectory.trajectory_type) << std::endl; return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED || breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED); } std::list AcrobaticSequence::getManeuverList() { return maneuver_list_; } } // namespace fpv_aggressive_trajectories ================================================ FILE: acrobatic_trajectory_helper/src/circle_trajectory_helper.cpp ================================================ #include "acrobatic_trajectory_helper/circle_trajectory_helper.h" #include #include namespace acrobatic_trajectory_helper { namespace circles { quadrotor_common::Trajectory computeVerticalCircleTrajectory( const Eigen::Vector3d center, const double orientation, const double radius, const double speed, const double phi_start, const double phi_end, const double sampling_frequency) { /* * We use a coordinate system with x to the front, y to the left, and z up. * Orientation is the angle by which the circle is rotated about the z-axis * If the orientation = 0.0 then the circle is in the x-z plane * When setting phi_start = 0.0 and orientation = 0.0, the start point will * be at (radius, 0, 0) + center * If phi_end > phi_start the circle is going in positive direction around * the y-axis rotated by orientation. Otherwise it is going in negative * direction */ quadrotor_common::Trajectory trajectory; trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; const double phi_total = phi_end - phi_start; const double direction = phi_total / fabs(phi_total); const double omega = direction * fabs(speed / radius); const double angle_step = fabs(omega / sampling_frequency); const Eigen::Quaterniond q_ori = Eigen::Quaterniond( Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation), Eigen::Vector3d::UnitZ())); for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) { const double phi = phi_start + direction * d_phi; const double cos_phi = cos(phi); const double sin_phi = sin(phi); quadrotor_common::TrajectoryPoint point; point.time_from_start = ros::Duration(fabs(d_phi / omega)); point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center; point.velocity = q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi)); point.acceleration = q_ori * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); point.jerk = q_ori * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi)); point.snap = q_ori * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0); Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors( I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) * Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0, std::sin(0.5 * orientation)); point.orientation = quatDes; // TODO: compute feedforward bodyrates double bodyrate_y = -omega; point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0); trajectory.points.push_back(point); } // Add last point at phi_end const double phi = phi_start + phi_total; const double cos_phi = cos(phi); const double sin_phi = sin(phi); quadrotor_common::TrajectoryPoint point; point.time_from_start = ros::Duration(fabs(phi_total / omega)); point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center; point.velocity = q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi)); point.acceleration = q_ori * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); point.jerk = q_ori * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi)); point.snap = q_ori * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0); Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors( I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) * Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0, std::sin(0.5 * orientation)); point.orientation = quatDes; trajectory.points.push_back(point); return trajectory; } quadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew( const Eigen::Vector3d center, const double orientation, const double radius, const double speed, const double phi_start, const double phi_end, const double corkscrew_velocity, const double sampling_frequency) { /* * We use a coordinate system with x to the front, y to the left, and z up. * Orientation is the angle by which the circle is rotated about the z-axis * If the orientation = 0.0 then the circle is in the x-z plane * When setting phi_start = 0.0 and orientation = 0.0, the start point will * be at (radius, 0, 0) + center * If phi_end > phi_start the circle is going in positive direction around * the y-axis rotated by orientation. Otherwise it is going in negative * direction */ quadrotor_common::Trajectory trajectory; trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; const double phi_total = phi_end - phi_start; const double direction = phi_total / fabs(phi_total); const double omega = direction * fabs(speed / radius); const double angle_step = fabs(omega / sampling_frequency); const double linear_step = fabs(corkscrew_velocity / sampling_frequency); double y_pos = 0.0; const Eigen::Quaterniond q_ori = Eigen::Quaterniond( Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation), Eigen::Vector3d::UnitZ())); for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) { const double phi = phi_start + direction * d_phi; const double cos_phi = cos(phi); const double sin_phi = sin(phi); quadrotor_common::TrajectoryPoint point; point.time_from_start = ros::Duration(fabs(d_phi / omega)); point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi)) + center; point.velocity = q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi)); point.acceleration = q_ori * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); point.jerk = q_ori * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi)); point.snap = q_ori * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0); Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) * Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0, std::sin(0.5 * orientation)); point.orientation = quatDes; // TODO: compute feedforward bodyrates double bodyrate_y = -omega; point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0); y_pos -= linear_step; trajectory.points.push_back(point); } // Add last point at phi_end const double phi = phi_start + phi_total; const double cos_phi = cos(phi); const double sin_phi = sin(phi); quadrotor_common::TrajectoryPoint point; point.time_from_start = ros::Duration(fabs(phi_total / omega)); point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi)) + center; point.velocity = q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi)); point.acceleration = q_ori * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); point.jerk = q_ori * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi)); point.snap = q_ori * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0); Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) * Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0, std::sin(0.5 * orientation)); point.orientation = quatDes; trajectory.points.push_back(point); return trajectory; } } // namespace circles } // namespace acrobatic_trajectory_helper ================================================ FILE: acrobatic_trajectory_helper/src/heading_trajectory_helper.cpp ================================================ #include "acrobatic_trajectory_helper/heading_trajectory_helper.h" #include namespace acrobatic_trajectory_helper { namespace heading { void addConstantHeading(const double heading, quadrotor_common::Trajectory* trajectory) { auto iterator(trajectory->points.begin()); auto iterator_prev(trajectory->points.begin()); iterator_prev = std::prev(iterator_prev); auto iterator_next(trajectory->points.begin()); iterator_next = std::next(iterator_next); auto last_element = trajectory->points.end(); last_element = std::prev(last_element); double time_step; for (int i = 0; i < trajectory->points.size(); i++) { // do orientation first, since bodyrate conversion will depend on it Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0); Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors( I_eZ_I, iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)); // set full orientation and heading to zero Eigen::Quaternion q_heading = Eigen::Quaternion(Eigen::AngleAxis( heading, Eigen::Matrix::UnitZ())); Eigen::Quaternion q_orientation = quatDes * q_heading; iterator->orientation = q_orientation; iterator->heading = 0.0; // heading is now absorbed in orientation iterator->heading_rate = 0.0; iterator->heading_acceleration = 0.0; Eigen::Vector3d thrust_1; Eigen::Vector3d thrust_2; // catch case of first and last element if (i == 0) { thrust_1 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81); time_step = (iterator_next->time_from_start - iterator->time_from_start).toSec(); thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81); } else if (i < trajectory->points.size() - 1) { thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81); time_step = (iterator_next->time_from_start - iterator_prev->time_from_start) .toSec(); thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81); } else { // at the last point, we extrapolate the acceleration thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81); thrust_2 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81) + time_step / 2.0 * iterator->jerk; } thrust_1.normalize(); thrust_2.normalize(); Eigen::Vector3d crossProd = thrust_1.cross(thrust_2); // direction of omega, in inertial axes Eigen::Vector3d angular_rates_wf = Eigen::Vector3d(0, 0, 0); if (crossProd.norm() > 0.0) { angular_rates_wf = std::acos(thrust_1.dot(thrust_2)) / time_step * crossProd / crossProd.norm(); } // rotate bodyrates to bodyframe iterator->bodyrates = q_orientation.inverse() * angular_rates_wf; iterator_prev++; iterator++; iterator_next++; } } void addConstantHeadingRate(const double initial_heading, const double final_heading, quadrotor_common::Trajectory* trajectory) { if (trajectory->points.size() < 2) { return; } const double delta_angle = quadrotor_common::wrapAngleDifference(initial_heading, final_heading); const double trajectory_duration = (trajectory->points.back().time_from_start - trajectory->points.front().time_from_start) .toSec(); const double heading_rate = delta_angle / trajectory_duration; std::list::iterator it; for (it = trajectory->points.begin(); it != trajectory->points.end(); it++) { const double duration_ratio = (it->time_from_start - trajectory->points.front().time_from_start) .toSec() / trajectory_duration; it->heading = initial_heading + duration_ratio * delta_angle; it->heading_rate = heading_rate; it->heading_acceleration = 0.0; } } } // namespace heading } // namespace acrobatic_trajectory_helper ================================================ FILE: acrobatic_trajectory_helper/src/polynomial_trajectory_helper.cpp ================================================ #include "acrobatic_trajectory_helper/polynomial_trajectory_helper.h" #include #include #include namespace acrobatic_trajectory_helper { namespace polynomials { // Constrained Polynomials quadrotor_common::Trajectory computeTimeOptimalTrajectory( const quadrotor_common::TrajectoryPoint& s0, const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::constrained_polynomial_trajectories:: computeTimeOptimalTrajectory(s0, s1, order_of_continuity, max_velocity, max_normalized_thrust, max_roll_pitch_rate); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory computeFixedTimeTrajectory( const quadrotor_common::TrajectoryPoint& s0, const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, const double execution_time, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::constrained_polynomial_trajectories:: computeFixedTimeTrajectory(s0, s1, order_of_continuity, execution_time); return samplePolynomial(polynomial, sampling_frequency); } // Minimum Snap Style Polynomials quadrotor_common::Trajectory generateMinimumSnapTrajectory( const Eigen::VectorXd& segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapTrajectory(segment_times, start_state, end_state, trajectory_settings); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapTrajectory( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapTrajectory(initial_segment_times, start_state, end_state, trajectory_settings, max_velocity, max_normalized_thrust, max_roll_pitch_rate); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapTrajectoryWithSegmentRefinement( initial_segment_times, start_state, end_state, trajectory_settings); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const quadrotor_common::TrajectoryPoint& start_state, const quadrotor_common::TrajectoryPoint& end_state, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapTrajectoryWithSegmentRefinement( initial_segment_times, start_state, end_state, trajectory_settings, max_velocity, max_normalized_thrust, max_roll_pitch_rate); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( const Eigen::VectorXd& segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapRingTrajectory(segment_times, trajectory_settings); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapRingTrajectory( initial_segment_times, trajectory_settings, max_velocity, max_normalized_thrust, max_roll_pitch_rate); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapRingTrajectoryWithSegmentRefinement( initial_segment_times, trajectory_settings); return samplePolynomial(polynomial, sampling_frequency); } quadrotor_common::Trajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( const Eigen::VectorXd& initial_segment_times, const polynomial_trajectories::PolynomialTrajectorySettings& trajectory_settings, const double max_velocity, const double max_normalized_thrust, const double max_roll_pitch_rate, const double sampling_frequency) { polynomial_trajectories::PolynomialTrajectory polynomial = polynomial_trajectories::minimum_snap_trajectories:: generateMinimumSnapRingTrajectoryWithSegmentRefinement( initial_segment_times, trajectory_settings, max_velocity, max_normalized_thrust, max_roll_pitch_rate); return samplePolynomial(polynomial, sampling_frequency); } // Sampling function quadrotor_common::Trajectory samplePolynomial( const polynomial_trajectories::PolynomialTrajectory& polynomial, const double sampling_frequency) { if (polynomial.trajectory_type == polynomial_trajectories::TrajectoryType::UNDEFINED) { return quadrotor_common::Trajectory(); } quadrotor_common::Trajectory trajectory; trajectory.points.push_back(polynomial.start_state); const ros::Duration dt(1.0 / sampling_frequency); ros::Duration time_from_start = polynomial.start_state.time_from_start + dt; while (time_from_start < polynomial.T) { trajectory.points.push_back(polynomial_trajectories::getPointFromTrajectory( polynomial, time_from_start)); time_from_start += dt; } trajectory.points.push_back(polynomial.end_state); trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; return trajectory; } } // namespace polynomials } // namespace acrobatic_trajectory_helper ================================================ FILE: conda_requirements.txt ================================================ defusedxml==0.6.0 docutils==0.15.2 google-auth-oauthlib==0.4.1 google-pasta==0.1.7 Keras-Applications==1.0.8 Keras-Preprocessing==1.1.0 oauthlib==3.1.0 pyasn1==0.4.8 pydot==1.4.1 requests-oauthlib==1.3.0 rsa==4.7 scipy==1.4.1 tensorflow-estimator==2.1.0 tensorflow-gpu==2.4.1 wrapt==1.11.2 ================================================ FILE: controller_learning/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(controller_learning) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) add_compile_options(-O3) #find_package(catkin_simple REQUIRED) #catkin_simple(ALL_DEPS_REQUIRED) #cs_add_library(acrobatic_sequence src/acrobatic_sequence.cpp) #cs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp) #target_link_libraries(fpv_aggressive_trajectories acrobatic_sequence) #cs_add_executable(odometry_republisher src/odometry_republisher.cpp) find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation catkin_simple REQUIRED ) catkin_python_setup() catkin_simple() cs_install() cs_export() ================================================ FILE: controller_learning/README.md ================================================ ================================================ FILE: controller_learning/common.py ================================================ import os import time def update_mpc_params(): params = {} params['Q_pos_xy'] = 200.0 params['Q_pos_z'] = 500.0 params['Q_attitude'] = 50.0 params['Q_velocity'] = 10.0 params['R_thrust'] = 0.1 params['R_pitchroll'] = 0.1 params['R_yaw'] = 0.1 params['state_cost_exp'] = 0.0 params['input_cost_exp'] = 0.0 params['max_bodyrate_xy'] = 20.0 params['max_bodyrate_z'] = 5.0 params['min_thrust'] = 1.0 params['max_thrust'] = 40.0 print("Setting control parameters of MPC to:") print(params) os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_pos_xy " + str(params['Q_pos_xy'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_pos_z " + str(params['Q_pos_z'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_attitude " + str(params['Q_attitude'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_velocity " + str(params['Q_velocity'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/R_thrust " + str(params['R_thrust'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/R_pitchroll " + str(params['R_pitchroll'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/R_yaw " + str(params['R_yaw'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/state_cost_exponential " + str(params['state_cost_exp'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/input_cost_exponential " + str(params['input_cost_exp'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_xy " + str(params['max_bodyrate_xy'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_z " + str(params['max_bodyrate_z'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/min_thrust " + str(params['min_thrust'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/max_thrust " + str(params['max_thrust'])) os.system("timeout 1s rostopic pub /hummingbird/autopilot/reload_parameters std_msgs/Empty '{}'") def setup_sim(): print("==========================") print(" RESET SIMULATION ") print("==========================") # set odometry converter back to ground truth state estimate # make sure fpv_aggressive_trajectories is not publishing anything! # turn off network os.system("timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: false'") os.system("timeout 1s rostopic pub /hummingbird/switch_to_network std_msgs/Bool 'data: false'") # after this message, autopilot will automatically go to 'BREAKING' and 'HOVER' state since # no control_command_inputs are published any more os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'") os.system("rosservice call /gazebo/pause_physics") print("Unpausing Physics...") os.system("rosservice call /gazebo/unpause_physics") print("Placing quadrotor...") os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty") os.system("rosservice call /gazebo/set_model_state " "'{model_state: { model_name: hummingbird, pose: { position: { x: 0.0, y: 0.0 ,z: 0.2 }, " "orientation: {x: 0, y: 0, z: 0.0, w: 1.0 }}, " "twist:{ linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 }}, " "reference_frame: world } }'") time.sleep(2) # start quadrotor # os.system("timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'") # os.system("timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty") def random_replace(): reset_success_str = 'rostopic pub /success_reset std_msgs/Empty "{}" -1' os.system(reset_success_str) def initialize_vio(): # Make sure to use GT odometry in this step os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty") os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'") # reset quad to initial position os.system("timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'") print("Start quadrotor") os.system("timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty") time.sleep(10) # Restart VIO os.system("timeout 1s rostopic pub /feature_tracker/restart std_msgs/Bool 'data: true' ") os.system( "timeout 1s rostopic pub /hummingbird/autopilot/pose_command geometry_msgs/PoseStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0} , frame_id: world}, pose:{position: { x: 0.0, y: 5.0, z: 4.0}, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0} } }'") time.sleep(9) # TODO: maybe remove this # os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Bool 'data: true'") return ================================================ FILE: controller_learning/config/dagger_settings.yaml ================================================ log_dir: "results/loop" quad_name: "hummingbird" verbose: False seq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency. checkpoint: # Put to true and add a path for loading a ckpt resume_training: False resume_file: "" data_generation: max_rollouts: 150 train_every_n_rollouts: 30 double_th_every_n_rollouts: 30 train: max_training_epochs: 20 max_allowed_error: 0.6 # Collision rollouts are eliminated from the training min_number_fts: 40 # Number of feature tracks per image batch_size: 32 summary_freq: 400 train_dir: "data/loop/train" # Where data will be generated and where the network will train val_dir: "data/loop/test" # Validation Data, change to a new data folder! save_every_n_epochs: 5 use_imu: True use_fts_tracks: True test_time: execute_nw_predictions: True # Dagger constants and random controller constants for exploration fallback_threshold_rates: 1.0 rand_thrust_mag: 6 rand_rate_mag: 3.5 rand_controller_prob: 0.05 ================================================ FILE: controller_learning/config/settings.py ================================================ import math import os import shutil import sys import time import datetime import yaml def create_settings(settings_yaml, mode='test'): setting_dict = {'train': TrainSetting, 'test': TestSetting, 'dagger': DaggerSetting} settings = setting_dict.get(mode, None) if settings is None: raise IOError("Unidentified Settings") settings = settings(settings_yaml) return settings class Settings: def __init__(self, settings_yaml, generate_log=True): assert os.path.isfile(settings_yaml), settings_yaml with open(settings_yaml, 'r') as stream: settings = yaml.safe_load(stream) self.quad_name = settings['quad_name'] self.seq_len = settings['seq_len'] # --- checkpoint --- checkpoint = settings['checkpoint'] self.resume_training = checkpoint['resume_training'] assert isinstance(self.resume_training, bool) self.resume_ckpt_file = checkpoint['resume_file'] # Save a copy of the parameters for reproducibility log_root = settings['log_dir'] if not log_root == '' and generate_log: current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S") self.log_dir = os.path.join(log_root, current_time) os.makedirs(self.log_dir) net_file = "./src/ControllerLearning/models/nets.py" assert os.path.isfile(net_file) shutil.copy(net_file, self.log_dir) shutil.copy(settings_yaml, self.log_dir) def add_flags(self): self._add_flags() def _add_flags(self): raise NotImplementedError class TrainSetting(Settings): def __init__(self, settings_yaml): super(TrainSetting, self).__init__(settings_yaml, generate_log=True) self.settings_yaml = settings_yaml self.add_flags() def _add_flags(self): with open(self.settings_yaml, 'r') as stream: settings = yaml.safe_load(stream) # --- Train Time --- # train_conf = settings['train'] self.max_training_epochs = train_conf['max_training_epochs'] self.max_allowed_error = train_conf['max_allowed_error'] self.batch_size = train_conf['batch_size'] self.summary_freq = train_conf['summary_freq'] self.train_dir = train_conf['train_dir'] self.use_fts_tracks = train_conf['use_fts_tracks'] self.use_imu = train_conf['use_imu'] self.val_dir = train_conf['val_dir'] self.min_number_fts = train_conf['min_number_fts'] self.save_every_n_epochs = train_conf['save_every_n_epochs'] class TestSetting(Settings): def __init__(self, settings_yaml): super(TestSetting, self).__init__(settings_yaml, generate_log=True) self.settings_yaml = settings_yaml self.add_flags() def _add_flags(self): with open(self.settings_yaml, 'r') as stream: settings = yaml.safe_load(stream) test_time = settings['test_time'] self.execute_nw_predictions = test_time['execute_nw_predictions'] assert isinstance(self.execute_nw_predictions, bool) self.max_rollouts = test_time['max_rollouts'] self.fallback_threshold_rates = test_time['fallback_threshold_rates'] self.fallback_threshold_thrust = test_time['fallback_threshold_thrust'] self.min_number_fts = test_time['min_number_fts'] self.use_imu = test_time['use_imu'] self.use_fts_tracks = test_time['use_fts_tracks'] self.verbose = settings['verbose'] class DaggerSetting(Settings): def __init__(self, settings_yaml): super(DaggerSetting, self).__init__(settings_yaml, generate_log=True) self.settings_yaml = settings_yaml self.add_flags() def _add_flags(self): with open(self.settings_yaml, 'r') as stream: settings = yaml.safe_load(stream) # --- Data Generation --- # data_gen = settings['data_generation'] self.max_rollouts = data_gen['max_rollouts'] self.double_th_every_n_rollouts = data_gen['double_th_every_n_rollouts'] self.train_every_n_rollouts = data_gen['train_every_n_rollouts'] # --- Test Time --- # test_time = settings['test_time'] self.execute_nw_predictions = test_time['execute_nw_predictions'] assert isinstance(self.execute_nw_predictions, bool) self.fallback_threshold_rates = test_time['fallback_threshold_rates'] self.rand_thrust_mag = test_time['rand_thrust_mag'] self.rand_rate_mag = test_time['rand_rate_mag'] self.rand_controller_prob = test_time['rand_controller_prob'] # --- Train Time --- # train_conf = settings['train'] self.max_training_epochs = train_conf['max_training_epochs'] self.max_allowed_error = train_conf['max_allowed_error'] self.batch_size = train_conf['batch_size'] self.min_number_fts = train_conf['min_number_fts'] self.summary_freq = train_conf['summary_freq'] self.train_dir = train_conf['train_dir'] self.val_dir = train_conf['val_dir'] self.use_imu = train_conf['use_imu'] self.use_fts_tracks = train_conf['use_fts_tracks'] self.val_dir = train_conf['val_dir'] self.save_every_n_epochs = train_conf['save_every_n_epochs'] self.verbose = settings['verbose'] assert isinstance(self.verbose, bool) ================================================ FILE: controller_learning/config/test_settings.yaml ================================================ quad_name: 'hummingbird' log_dir: '/tmp/test' ideal_inputs: False seq_len: 1 verbose: False throw_params: vlin_range: 5 vang_range: 5 pos_range: 5 checkpoint: resume_training: True resume_file: "ckpt/loop/fts_and_imu/ckpt-7" test_time: max_rollouts: 5 min_number_fts: 40 execute_nw_predictions: True fallback_threshold_thrust: 100 fallback_threshold_rates: 100 use_imu: True use_fts_tracks: True ================================================ FILE: controller_learning/config/train_settings.yaml ================================================ log_dir: 'results/train_fma_cork_seq_1_fts_tracks_newcam_new_end' quad_name: 'none' seq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency. checkpoint: # Put to true and add a path for loading a ckpt resume_training: False resume_file: "" train: max_training_epochs: 150 max_allowed_error: 0.7 batch_size: 32 summary_freq: 400 min_number_fts: 40 train_dir: "data/loop/train" val_dir: "data/loop/val" save_every_n_epochs: 5 use_imu: True use_fts_tracks: True ================================================ FILE: controller_learning/data/.gitignore ================================================ * */ !.gitignore ================================================ FILE: controller_learning/iterative_learning_trajectories.py ================================================ #!/usr/bin/env python3 import argparse import os import sys import time import numpy as np import rospy from ControllerLearning import TrajectoryLearning from std_msgs.msg import Bool from common import update_mpc_params, setup_sim, random_replace, initialize_vio from config.settings import create_settings class Trainer(): def __init__(self, settings): rospy.init_node('iterative_learning_node', anonymous=False) self.settings = settings self.trajectory_done = False self.traj_done_sub = rospy.Subscriber("/hummingbird/switch_to_network", Bool, self.callback_traj_done, queue_size=1) def callback_traj_done(self, data): self.trajectory_done = data.data def start_experiment(self, learner): reset_success_str = 'rostopic pub /success_reset std_msgs/Empty "{}" -1' os.system(reset_success_str) initialize_vio() learner.latest_thrust_factor = 1.0 # Could be changed to adapt for different quadrotors. print("Doing experiment {}, with such factor {}".format(learner.rollout_idx, learner.latest_thrust_factor)) # if True, we will still use the VIO-orientation, even when initialization is poor. # If set to False, we will fall back to GT. # Set to True only if you are sure your VIO is well calibrated. use_chimaera = False # check if initialization was good. If not, we will perform rollout with ground truth to not waste time! vio_init_good = learner.vio_init_good if vio_init_good: rospy.loginfo("VINS-Mono initialization is good, switching to vision-based state estimate!") os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 1'") else: if use_chimaera: rospy.logerr("VINS-Mono initialization is poor, use orientation, bodyrates from VIO and linear velocity estimate from GT!") os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 2'") else: rospy.logerr("VINS-Mono initialization is poor, keeping ground truth estimate!") os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'") # Start Flying! os.system("timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: true'") return vio_init_good def perform_training(self): learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode="iterative") shutdown_requested = False train_every_n_rollouts = self.settings.train_every_n_rollouts if self.settings.execute_nw_predictions: print("-------------------------------------------") print("Running Dagger with the following params") print("Rates threshold: {}; Rand Controller Th {}".format( self.settings.fallback_threshold_rates, self.settings.rand_controller_prob)) print("-------------------------------------------") else: print("---------------------------") print("Collecting Data with Expert") print("---------------------------") while (not shutdown_requested) and (learner.rollout_idx < self.settings.max_rollouts): self.trajectory_done = False setup_sim() learner.start_data_recording() self.start_experiment(learner) print("Starting Experiment {}".format(learner.rollout_idx)) start_time = time.time() time_run = 0 ref_log = [] gt_pos_log = [] error_log = [] while (not self.trajectory_done) and (time_run < 100): time.sleep(0.1) time_run = time.time() - start_time if learner.use_network and learner.reference_updated: pos_ref_dict = learner.compute_trajectory_error() gt_pos_log.append(pos_ref_dict["gt_pos"]) ref_log.append(pos_ref_dict["gt_ref"]) error_log.append(np.linalg.norm(pos_ref_dict["gt_pos"] - pos_ref_dict["gt_ref"])) # final logging tracking_error = np.mean(error_log) median_traj_error = np.median(error_log) t_log = np.stack((ref_log, gt_pos_log), axis=0) expert_usage = learner.stop_data_recording() shutdown_requested = learner.shutdown_requested() print("Expert used {:.03f}% of the times".format(100.0 * expert_usage)) print("Mean Tracking Error is {:.03f}".format(tracking_error)) print("Median Tracking Error is {:.03f}".format(median_traj_error)) if learner.rollout_idx % train_every_n_rollouts == 0: os.system("rosservice call /gazebo/pause_physics") learner.train() os.system("rosservice call /gazebo/unpause_physics") if (learner.rollout_idx % self.settings.double_th_every_n_rollouts) == 0: self.settings.fallback_threshold_rates += 0.5 print("Setting Rate Threshold to {}".format(self.settings.fallback_threshold_rates)) self.settings.rand_controller_prob = np.minimum(0.3, self.settings.rand_controller_prob * 2) print("Setting Rand Controller Prob to {}".format(self.settings.rand_controller_prob)) if self.settings.verbose: t_log_fname = os.path.join(self.settings.log_dir, "traj_log_{:5d}.npy".format(learner.rollout_idx)) np.save(t_log_fname, t_log) def perform_testing(self): learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode="testing") shutdown_requested = False rollout_idx = 0 while (not shutdown_requested) and (rollout_idx < self.settings.max_rollouts): self.trajectory_done = False setup_sim() if self.settings.verbose: # Will save data for debugging reasons learner.start_data_recording() self.start_experiment(learner) start_time = time.time() time_run = 0 ref_log = [] gt_pos_log = [] error_log = [] while (not self.trajectory_done) and (time_run < 100): time.sleep(0.1) time_run = time.time() - start_time if learner.use_network and learner.reference_updated: pos_ref_dict = learner.compute_trajectory_error() gt_pos_log.append(pos_ref_dict["gt_pos"]) ref_log.append(pos_ref_dict["gt_ref"]) error_log.append(np.linalg.norm(pos_ref_dict["gt_pos"] - pos_ref_dict["gt_ref"])) # final logging tracking_error = np.mean(error_log) median_traj_error = np.median(error_log) t_log = np.stack((ref_log, gt_pos_log), axis=0) expert_usage = learner.stop_data_recording() shutdown_requested = learner.shutdown_requested() print("{} Rollout: Expert used {:.03f}% of the times".format(rollout_idx+1, 100.0 * expert_usage)) print("Mean Tracking Error is {:.03f}".format(tracking_error)) print("Median Tracking Error is {:.03f}".format(median_traj_error)) rollout_idx +=1 if self.settings.verbose: t_log_fname = os.path.join(self.settings.log_dir, "traj_log_{:05d}.npy".format(rollout_idx)) np.save(t_log_fname, t_log) def main(): parser = argparse.ArgumentParser(description='Train RAF network.') parser.add_argument('--settings_file', help='Path to settings yaml', required=True) args = parser.parse_args() settings_filepath = args.settings_file settings = create_settings(settings_filepath, mode='dagger') update_mpc_params() setup_sim() trainer = Trainer(settings) trainer.perform_training() if __name__ == "__main__": main() ================================================ FILE: controller_learning/launch/simulation/base_quad_simulation_no_controller.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/controller.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/custom_rotors_interface.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/minisim.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/quadrotor_empty_world_no_controller.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/quadrotor_rgb_world_no_controller.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/rpg_mpc.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/rpg_pid.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/rpg_rotors_interface.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/rviz_only.launch ================================================ ================================================ FILE: controller_learning/launch/simulation/simulation.launch ================================================ ================================================ FILE: controller_learning/package.xml ================================================ controller_learning 0.0.0 The controller_learning package Elia Kaufmann TODO Elia Kaufmann catkin catkin_simple autopilot eigen_catkin minimum_jerk_trajectories quadrotor_common quadrotor_msgs roscpp std_msgs nav_msgs ================================================ FILE: controller_learning/results/.gitignore ================================================ * */ !.gitignore ================================================ FILE: controller_learning/setup.py ================================================ #!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['ControllerLearning'], package_dir={'': 'src'} ) setup(**d) ================================================ FILE: controller_learning/src/ControllerLearning/TrajectoryBase.py ================================================ #!/usr/bin/env python3 import collections import copy import csv import datetime import os import random import numpy as np import rospy from nav_msgs.msg import Odometry from quadrotor_msgs.msg import ControlCommand from quadrotor_msgs.msg import TrajectoryPoint from sensor_msgs.msg import PointCloud from std_msgs.msg import Bool from std_msgs.msg import Empty from scipy.spatial.transform import Rotation as R from .models.bodyrate_learner import BodyrateLearner TRACK_NUM_NORMALIZE = 10 # Normalization factor for feature length class TrajectoryBase(object): def __init__(self, config, mode): self.config = config self.odometry = Odometry() self.gt_odometry = Odometry() self.rows_buffer = [] self.ref_state = TrajectoryPoint() self.vins_odometry = None self.gt_control_command = ControlCommand() self.counter = 0 self.features = None self.image = None self.images_input = None self.maneuver_complete = False self.shutdown_node = False self.record_data = False self.is_training = False self.use_network = False self.net_initialized = False self.reference_updated = False self.rollout_idx = 0 self.n_times_net = 0.001 # Due to cope against weird gazebo behaviour self.n_times_expert = 0 self.mode = mode self.fts_queue = collections.deque([], maxlen=self.config.seq_len) self.state_queue = collections.deque([], maxlen=self.config.seq_len) self.reset_queue() self.learner = BodyrateLearner(settings=config) if self.mode == 'training': return # Nothing to initialize self.pub_actions = rospy.Publisher("/" + self.config.quad_name + "/control_command", ControlCommand, queue_size=1) self.odometry_sub = rospy.Subscriber("/" + self.config.quad_name + "/state_estimate", Odometry, self.callback_odometry, queue_size=1, tcp_nodelay=True) self.shutdown_sub = rospy.Subscriber("shutdown_learner", Empty, self.callback_shutdown, queue_size=1) self.ref_sub = rospy.Subscriber("/" + self.config.quad_name + "/vio_reference", TrajectoryPoint, self.callback_ref, queue_size=1, tcp_nodelay=True) self.control_command_sub = rospy.Subscriber("/" + self.config.quad_name + "/control_command_label", ControlCommand, self.callback_control_command, queue_size=1, tcp_nodelay=True) if self.config.use_fts_tracks or self.mode == 'iterative': self.fts_sub = rospy.Subscriber("/feature_tracker/feature", PointCloud, self.callback_fts, queue_size=1) self.traj_done_sub = rospy.Subscriber("/" + self.config.quad_name + "/switch_to_network", Bool, self.callback_nw_switch, queue_size=1) self.trajectory_start = rospy.Subscriber("/" + self.config.quad_name + "/trajectory_computation_finish", Bool, self.callback_start_trajectory, queue_size=10) if self.mode == "testing": self.success = 1 def start_data_recording(self): print("Collecting data") self.record_data = True def stop_data_recording(self): print("Stop data collection") self.record_data = False expert_usage = self.n_times_expert / (self.n_times_net + self.n_times_expert) return expert_usage def reset_queue(self): self.fts_queue.clear() self.state_queue.clear() self.ref_rot = [0 for _ in range(9)] self.odom_rot = [0 for _ in range(9)] init_dict = {} if self.config.use_imu: n_init_states = 30 else: n_init_states = 15 for i in range(self.config.min_number_fts): init_dict[i] = np.zeros((5,), dtype=np.float32) for _ in range(self.config.seq_len): self.fts_queue.append(init_dict) self.state_queue.append(np.zeros((n_init_states,))) self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \ for j in range(self.config.seq_len)]) def publish_control_command(self, control_command): self.pub_actions.publish(control_command) def preprocess_fts(self, data): features_dict = {} for i in range(len(data.points)): ft_id = data.channels[0].values[i] x = data.points[i].x y = data.points[i].y z = data.points[i].z velocity_x = data.channels[3].values[i] velocity_y = data.channels[4].values[i] track_count = 2 * (data.channels[5].values[i] / TRACK_NUM_NORMALIZE) - 1 assert z == 1 feat = np.array([x, y, velocity_x, velocity_y, track_count]) features_dict[ft_id] = feat return features_dict def add_missing_fts(self, features_dict): processed_dict = copy.copy(features_dict) # Could be both positive or negative missing_fts = self.config.min_number_fts - len(features_dict.keys()) if missing_fts > 0: # Features are missing if missing_fts != self.config.min_number_fts: # There is something, we can sample new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts)) for j in range(missing_fts): processed_dict[-j - 1] = features_dict[new_features_keys[j]] else: raise IOError("There should not be zero features!") elif missing_fts < 0: # There are more features than we need, so sample del_features_keys = random.sample(features_dict.keys(), int(-missing_fts)) for k in del_features_keys: del processed_dict[k] return processed_dict def callback_fts(self, data): if (not self.config.use_fts_tracks) and (self.mode == 'testing'): return features = self.preprocess_fts(data) if len(features.keys()) != 0: # Update features only if something is available self.features = features preprocessed_fts = self.add_missing_fts(self.features) self.fts_queue.append(preprocessed_fts) self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \ for j in range(self.config.seq_len)]) def callback_shutdown(self, data): self.shutdown_node = True def callback_nw_switch(self, msg): self.use_network = False if msg.data: # Trajectory is done, stop everything self.maneuver_complete = True print("Maneuver is finished") def callback_start_trajectory(self, data): # VIO is ready, can fly new trajectory if data.data: print("Ready to start trajectory, network on") self.use_network = True self.counter = 0 def callback_start(self, data): print("Callback START") self.pipeline_off = False def callback_off(self, data): print("Callback OFF") self.pipeline_off = True def maneuver_finished(self): return self.maneuver_complete def callback_odometry(self, data): self.odometry = data self.odom_rot = R.from_quat([self.odometry.pose.pose.orientation.x, self.odometry.pose.pose.orientation.y, self.odometry.pose.pose.orientation.z, self.odometry.pose.pose.orientation.w]).as_matrix().reshape((9,)).tolist() def callback_gt_odometry(self, data): self.gt_odometry = data def callback_ref(self, data): self.ref_state = data self.ref_rot = R.from_quat([self.ref_state.pose.orientation.x, self.ref_state.pose.orientation.y, self.ref_state.pose.orientation.z, self.ref_state.pose.orientation.w]).as_matrix().reshape((9,)).tolist() if not self.reference_updated: self.reference_updated = True def callback_control_command(self, data): self.control_command = data self._generate_control_command() def shutdown_requested(self): return self.shutdown_node def _prepare_net_inputs(self): if not self.net_initialized: # return fake input for init # TODO: change to features if self.config.use_imu: n_init_states = 30 else: n_init_states = 15 inputs = {'fts': np.zeros((1, self.config.seq_len, 40, 5), dtype=np.float64), 'state': np.zeros((1, self.config.seq_len, n_init_states), dtype=np.float64)} return inputs # Reference state_inputs = self.ref_rot + [ self.ref_state.velocity.linear.x, self.ref_state.velocity.linear.y, self.ref_state.velocity.linear.z, self.ref_state.velocity.angular.x, self.ref_state.velocity.angular.y, self.ref_state.velocity.angular.z] if self.config.use_imu: imu_states = self.odom_rot + [ self.odometry.twist.twist.linear.x, self.odometry.twist.twist.linear.y, self.odometry.twist.twist.linear.z, self.odometry.twist.twist.angular.x, self.odometry.twist.twist.angular.y, self.odometry.twist.twist.angular.z] state_inputs = imu_states + state_inputs state_inputs = np.array(state_inputs) self.state_queue.append(state_inputs) state_inputs = np.stack(self.state_queue, axis=0) inputs = {'fts': np.expand_dims(self.features_input, axis=0), 'state': np.expand_dims(state_inputs, axis=0)} return inputs def _generate_control_command(self): pass # Implemented in derived class def write_csv_header(self): pass # Implemented in derived class ================================================ FILE: controller_learning/src/ControllerLearning/TrajectoryLearning.py ================================================ #!/usr/bin/env python3 import csv import datetime import os import random import numpy as np import rospy from nav_msgs.msg import Odometry from quadrotor_msgs.msg import ControlCommand from std_msgs.msg import Bool from std_msgs.msg import Empty from .TrajectoryBase import TrajectoryBase, TRACK_NUM_NORMALIZE class TrajectoryLearning(TrajectoryBase): def __init__(self, config, mode): super(TrajectoryLearning, self).__init__(config, mode) self.gt_odometry = Odometry() self.latest_thrust_factor = 1.0 # Init, can be changed for different drones self.recorded_samples = 0 if self.mode == 'training': return # Nothing to initialize self.pub_reset_vio = rospy.Publisher("/feature_tracker/restart", Bool, queue_size=1) self.success_subs = rospy.Subscriber("success_reset", Empty, self.callback_success_reset, queue_size=1) self.state_estimate_sub = rospy.Subscriber("/hummingbird/odometry_converted_vio", Odometry, self.callback_odometry, queue_size=1, tcp_nodelay=True) self.ground_truth_odom = rospy.Subscriber("/hummingbird/ground_truth/odometry", Odometry, self.callback_gt_odometry, queue_size=1) self.vins_mono_sub = rospy.Subscriber("/vins_estimator/imu_propagate", Odometry, self.callback_vins_mono, queue_size=1) if mode == "iterative" or self.config.verbose: self.write_csv_header() if self.mode == "testing": self.success = 1 def train(self): self.is_training = True self.learner.train() self.is_training = False self.use_network = False def callback_success_reset(self, data): print("Received call to Clear Buffer and Restart Experiment") os.system("rosservice call /gazebo/pause_physics") self.rollout_idx += 1 self.reset_queue() print('Buffer Cleared') if self.mode == 'testing': self.success = 1 # We are positive, default is pass # Init is hacky, but gazebo is very bad! self.n_times_expert = 0.000 self.n_times_net = 0.001 self.reference_updated = False os.system("rosservice call /gazebo/unpause_physics") print('Done Reset') def callback_gt_odometry(self, data): self.gt_odometry = data def callback_vins_mono(self, data): self.vins_odometry = data @property def vio_init_good(self): max_allowed_velocity = 0.1 if abs(self.vins_odometry.twist.twist.linear.x) < max_allowed_velocity and \ abs(self.vins_odometry.twist.twist.linear.y) < max_allowed_velocity and \ abs(self.vins_odometry.twist.twist.linear.z) < max_allowed_velocity: return True else: return False def save_data(self): row = [self.rollout_idx, self.odometry.header.stamp.to_sec(), # GT Positon self.gt_odometry.pose.pose.position.x, self.gt_odometry.pose.pose.position.y, self.gt_odometry.pose.pose.position.z, self.ref_state.pose.position.z - self.gt_odometry.pose.pose.position.z, self.gt_odometry.pose.pose.orientation.w, self.gt_odometry.pose.pose.orientation.x, self.gt_odometry.pose.pose.orientation.y, self.gt_odometry.pose.pose.orientation.z, self.gt_odometry.twist.twist.linear.x, self.gt_odometry.twist.twist.linear.y, self.gt_odometry.twist.twist.linear.z, self.gt_odometry.twist.twist.angular.x, self.gt_odometry.twist.twist.angular.y, self.gt_odometry.twist.twist.angular.z, # VIO Estimate self.odometry.pose.pose.position.x, self.odometry.pose.pose.position.y, self.odometry.pose.pose.position.z, self.ref_state.pose.position.z - self.odometry.pose.pose.position.z, self.odometry.pose.pose.orientation.w, self.odometry.pose.pose.orientation.x, self.odometry.pose.pose.orientation.y, self.odometry.pose.pose.orientation.z, self.odometry.twist.twist.linear.x, self.odometry.twist.twist.linear.y, self.odometry.twist.twist.linear.z, self.odometry.twist.twist.angular.x, self.odometry.twist.twist.angular.y, self.odometry.twist.twist.angular.z, # Reference state self.ref_state.pose.position.x, self.ref_state.pose.position.y, self.ref_state.pose.position.z, self.ref_state.pose.orientation.w, self.ref_state.pose.orientation.x, self.ref_state.pose.orientation.y, self.ref_state.pose.orientation.z, self.ref_state.velocity.linear.x, self.ref_state.velocity.linear.y, self.ref_state.velocity.linear.z, self.ref_state.velocity.angular.x, self.ref_state.velocity.angular.y, self.ref_state.velocity.angular.z, # MPC output with GT Position self.control_command.collective_thrust, self.control_command.bodyrates.x, self.control_command.bodyrates.y, self.control_command.bodyrates.z, # NET output with GT Position self.net_control.collective_thrust, self.net_control.bodyrates.x, self.net_control.bodyrates.y, self.net_control.bodyrates.z, # Maneuver type 0] if self.record_data and self.gt_odometry.pose.pose.position.z > 0.3 and \ self.control_command.collective_thrust > 0.2: with open(self.csv_filename, 'a') as writeFile: writer = csv.writer(writeFile) writer.writerows([row]) fts_name = '{:08d}.npy' fts_filename = os.path.join(self.image_save_dir, fts_name.format(self.recorded_samples)) np.save(fts_filename, self.features) self.recorded_samples += 1 def callback_control_command(self, data): self.control_command = data self._generate_control_command() if self.mode == 'testing' and self.gt_odometry.pose.pose.position.z < 0.3: self.success = 0 def compute_trajectory_error(self): gt_ref = np.array([self.ref_state.pose.position.x, self.ref_state.pose.position.y, self.ref_state.pose.position.z]) gt_pos = np.array([self.gt_odometry.pose.pose.position.x, self.gt_odometry.pose.pose.position.y, self.gt_odometry.pose.pose.position.z]) results = {"gt_ref": gt_ref, "gt_pos": gt_pos} return results def publish_control_command(self, control_command): control_command.collective_thrust = self.latest_thrust_factor * control_command.collective_thrust self.pub_actions.publish(control_command) def _generate_control_command(self): inputs = self._prepare_net_inputs() if not self.net_initialized: # Apply Network to init results = self.learner.inference(inputs) print("Net initialized") self.net_initialized = True self.publish_control_command(self.control_command) if not self.use_network or not self.reference_updated \ or (len(inputs['fts'].shape) != 4): # Will be in here if: # - starting and VIO init # - Image queue is not ready, can only run expert self.publish_control_command(self.control_command) if self.use_network: print("Using expert wait for ref") return # Use always expert at the beginning (approximately 0.2s) to avoid syncronization problems if self.counter < 10: self.counter += 1 self.publish_control_command(self.control_command) return # Apply Network results = self.learner.inference(inputs) control_command = ControlCommand() control_command.armed = True control_command.expected_execution_time = rospy.Time.now() control_command.control_mode = 2 control_command.collective_thrust = results[0][0].numpy() control_command.bodyrates.x = results[0][1].numpy() control_command.bodyrates.y = results[0][2].numpy() control_command.bodyrates.z = results[0][3].numpy() self.net_control = control_command # Log immediately everything to avoid surprises (if required) if self.record_data: self.save_data() # Apply random controller now and then to facilitate exploration if (self.mode != 'testing') and random.random() < self.config.rand_controller_prob: self.control_command.collective_thrust += self.config.rand_thrust_mag * (random.random() - 0.5) * 2 self.control_command.bodyrates.x += self.config.rand_rate_mag * (random.random() - 0.5) * 2 self.control_command.bodyrates.y += self.config.rand_rate_mag * (random.random() - 0.5) * 2 self.control_command.bodyrates.z += self.config.rand_rate_mag * (random.random() - 0.5) * 2 self.publish_control_command(self.control_command) return # Dagger (on control command label). d_thrust = control_command.collective_thrust - self.control_command.collective_thrust d_br_x = control_command.bodyrates.x - self.control_command.bodyrates.x d_br_y = control_command.bodyrates.y - self.control_command.bodyrates.y d_br_z = control_command.bodyrates.z - self.control_command.bodyrates.z if self.config.execute_nw_predictions \ and abs(d_thrust) < self.config.fallback_threshold_rates \ and abs(d_br_x) < self.config.fallback_threshold_rates \ and abs(d_br_y) < self.config.fallback_threshold_rates \ and abs(d_br_z) < self.config.fallback_threshold_rates: self.n_times_net += 1 self.publish_control_command(control_command) else: self.n_times_expert += 1 self.publish_control_command(self.control_command) def write_csv_header(self): row = ["Rollout_idx", "Odometry_stamp", # GT Position "gt_Position_x", "gt_Position_y", "gt_Position_z", "gt_Position_z_error", "gt_Orientation_w", "gt_Orientation_x", "gt_Orientation_y", "gt_Orientation_z", "gt_V_linear_x", "gt_V_linear_y", "gt_V_linear_z", "gt_V_angular_x", "gt_V_angular_y", "gt_V_angular_z", # VIO Estimate "Position_x", "Position_y", "Position_z", "Position_z_error", "Orientation_w", "Orientation_x", "Orientation_y", "Orientation_z", "V_linear_x", "V_linear_y", "V_linear_z", "V_angular_x", "V_angular_y", "V_angular_z", # Reference state "Reference_position_x", "Reference_position_y", "Reference_position_z", "Reference_orientation_w", "Reference_orientation_x", "Reference_orientation_y", "Reference_orientation_z", "Reference_v_linear_x", "Reference_v_linear_y", "Reference_v_linear_z", "Reference_v_angular_x", "Reference_v_angular_y", "Reference_v_angular_z", # MPC output with GT Postion "Gt_control_command_collective_thrust", "Gt_control_command_bodyrates_x", "Gt_control_command_bodyrates_y", "Gt_control_command_bodyrates_z", # Net output "Net_control_command_collective_thrust", "Net_control_command_bodyrates_x", "Net_control_command_bodyrates_y", "Net_control_command_bodyrates_z", "Maneuver_type"] current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S") if self.mode == 'iterative': root_save_dir = self.config.train_dir else: root_save_dir = self.config.log_dir self.csv_filename = os.path.join(root_save_dir, "data_" + current_time + ".csv") self.image_save_dir = os.path.join(root_save_dir, "img_data_" + current_time) if not os.path.exists(self.image_save_dir): os.makedirs(self.image_save_dir) with open(self.csv_filename, 'w') as writeFile: writer = csv.writer(writeFile) writer.writerows([row]) ================================================ FILE: controller_learning/src/ControllerLearning/__init__.py ================================================ ================================================ FILE: controller_learning/src/ControllerLearning/models/__init__.py ================================================ ================================================ FILE: controller_learning/src/ControllerLearning/models/body_dataset.py ================================================ import numpy as np import glob import os import pandas as pd import tensorflow as tf import fnmatch import cv2 import random from scipy.spatial.transform import Rotation as R def create_dataset(directory, settings, training=True): dataset = SafeDataset(directory, settings, training) return dataset class BodyDataset: """ Base Dataset Class """ def __init__(self, directory, config, training=True): self.config = config self.directory = directory self.training = training self.samples = 0 self.experiments = [] self.features = [] self.labels = [] self.filenames = [] self.stacked_filenames = [] # Will be used for passing stacked fnames img_rootname = 'img_data' for root, dirs, files in os.walk(directory, topdown=True, followlinks=True): for name in dirs: if name.startswith(img_rootname): exp_dir = os.path.join(root, name) self.experiments.append(os.path.abspath(exp_dir)) self.num_experiments = len(self.experiments) self.img_format = 'npy' self.data_format = 'csv' for exp_dir in self.experiments: try: self._decode_experiment_dir(exp_dir) except: raise ImportWarning("Image reading in {} failed".format( exp_dir)) if self.samples == 0: raise IOError("Did not find any file in the dataset folder") print('Found {} images belonging to {} experiments:'.format( self.samples, self.num_experiments)) def _recursive_list(self, subpath, fmt='jpg'): return fnmatch.filter(os.listdir(subpath), '*.{}'.format(fmt)) def build_dataset(self): self._build_dataset() def _build_dataset(self): raise NotImplementedError def _decode_experiment_dir(self, directory): raise NotImplementedError class SafeDataset(BodyDataset): def __init__(self, directory, config, training=True): super(SafeDataset, self).__init__(directory, config, training) self.build_dataset() def _decode_experiment_dir(self, dir_subpath): base_path = os.path.basename(dir_subpath) parent_dict = os.path.dirname(dir_subpath) data_name = 'data' + base_path[8:] + ".csv" data_name = os.path.join(parent_dict, data_name) assert os.path.isfile(data_name), "Not Found data file" df = pd.read_csv(data_name, delimiter=',') num_files = df.shape[0] num_images = len(self._recursive_list(dir_subpath, fmt=self.img_format)) assert num_files == num_images, "Number of features and images does not match" features_imu = [# VIO Estimate "Orientation_x", "Orientation_y", "Orientation_z", "Orientation_w", "V_linear_x", "V_linear_y", "V_linear_z", "V_angular_x", "V_angular_y", "V_angular_z"] features = [# Reference state "Reference_orientation_x", "Reference_orientation_y", "Reference_orientation_z", "Reference_orientation_w", "Reference_v_linear_x", "Reference_v_linear_y", "Reference_v_linear_z", "Reference_v_angular_x", "Reference_v_angular_y", "Reference_v_angular_z"] # Preprocessing: we select the good rollouts (no crash in training data) rollout_fts = ["Rollout_idx"] rollout_fts_v = df[rollout_fts].values position_gt = ["gt_Position_x", "gt_Position_y", "gt_Position_z"] position_gt_v = df[position_gt].values position_ref = ["Reference_position_x", "Reference_position_y", "Reference_position_z"] position_ref_v = df[position_ref].values good_rollouts = [] if rollout_fts_v.shape[0] == 0: return for r in np.arange(1,np.max(rollout_fts_v)+1): rollout_positions = rollout_fts_v == r roll_gt = position_gt_v[np.squeeze(rollout_positions),:] roll_ref = position_ref_v[np.squeeze(rollout_positions),:] if roll_gt.shape[0] == 0: continue assert roll_ref.shape == roll_gt.shape error = np.mean(np.linalg.norm(roll_gt - roll_ref, axis=1)) if error < self.config.max_allowed_error: good_rollouts.append(r) if self.config.use_imu: features = ["Rollout_idx"] + features_imu + features else: features = ["Rollout_idx"] + features labels = ["Gt_control_command_collective_thrust", "Gt_control_command_bodyrates_x", "Gt_control_command_bodyrates_y", "Gt_control_command_bodyrates_z"] features_v = df[features].values labels_v = df[labels].values for frame_number in range(num_files): is_valid = False img_fname = os.path.join(dir_subpath, "{:08d}.{}".format(frame_number, self.img_format)) if os.path.isfile(img_fname) and (rollout_fts_v[frame_number] in good_rollouts): is_valid = True if is_valid: self.features.append(self.preprocess_fts(features_v[frame_number])) self.labels.append(labels_v[frame_number]) if self.config.use_fts_tracks: self.filenames.append(img_fname) self.samples += 1 def preprocess_fts(self, fts): """ Converts rotations from quadrans to rotation matrix. Fts have the following indexing. rollout_idx, qx,qy,qz,qw, vx, vy, vz, ax, ay, az, rqx, rqy, rqz, rqw, ... """ fts = fts.tolist() ref_rot = R.from_quat(fts[11:15]).as_matrix().reshape((9,)).tolist() if self.config.use_imu: odom_rot = R.from_quat(fts[1:5]).as_matrix().reshape((9,)).tolist() processed_fts = [fts[0]] + odom_rot + fts[5:11] + ref_rot + fts[15:] else: processed_fts = [fts[0]] + ref_rot + fts[5:11] return np.array(processed_fts) def add_missing_fts(self, features_dict): processed_dict = features_dict # Could be both positive or negative missing_fts = self.config.min_number_fts - len(features_dict.keys()) if missing_fts > 0: # Features are missing if missing_fts != self.config.min_number_fts: # There is something, we can sample new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts)) for j in range(missing_fts): processed_dict[-j-1] = features_dict[new_features_keys[j]] else: # Zero features, this is a transient for j in range(missing_fts): processed_dict[-j-1] = np.zeros((5,)) elif missing_fts < 0: # There are more features than we need, so sample del_features_keys = random.sample(features_dict.keys(), int(-missing_fts)) for k in del_features_keys: del processed_dict[k] return processed_dict def load_fts_sequence(self, sample_num): fts_seq = [] sample_num_np = sample_num.numpy() filenames_num = self.stacked_filenames[sample_num_np] for idx in range(self.config.seq_len): fname_idx = filenames_num[idx] if fname_idx < 0: fts = {} else: fname = self.filenames[fname_idx] fts = np.load(fname, allow_pickle=True).item() fts_seq.append(fts) # Reverse list to have it ordered in time (t-seq_len, ..., t) fts_seq = reversed(fts_seq) # Crop to the required lenght fts_seq = [self.add_missing_fts(ft) for ft in fts_seq] # Stack features_input = np.stack([np.stack([v for v in fts_seq[j].values()]) \ for j in range(self.config.seq_len)]) return features_input def _dataset_map(self, sample_num): # First is rollout idx label = tf.gather(self.labels, sample_num) state_seq = [] # For states is easy: nothing to do. for idx in reversed(range(self.config.seq_len)): state = tf.gather(self.features, sample_num - idx)[1:] state_seq.append(state) state_seq = tf.stack(state_seq) # For images, take care they do not overlap if self.config.use_fts_tracks: fts_seq = tf.py_function(func=self.load_fts_sequence, inp=[sample_num], Tout=tf.float32) return (state_seq, fts_seq), label else: return state_seq, label def check_equal_dict(self, d1, d2): for k_1, v_1 in d1.items(): try: v_2 = d2[k_1] except: return False if not np.array_equal(v_1,v_2): return False return True def _preprocess_fnames(self): # Append filenames up to seq_len for fast loading. # A bit ugly and inefficent, can be improved self.last_init_fts = None for k in range(len(self.filenames)): if k % 3000 == 0: print("Built {:.2f}% of the dataset".format( k/len(self.filenames)*100), end='\r') # Check if you can copy the things before kth_fts = self.filenames[k] kth_fts = np.load(kth_fts, allow_pickle=True).item() if k > 0: if self.check_equal_dict(self.last_init_fts, kth_fts): self.stacked_filenames.append(self.stacked_filenames[-1]) continue # This is the lastest observed feature track different from others self.last_init_fts = kth_fts idx = 0 rollout_idxes = [] fname_seq = [] fts_seq = [] while len(fts_seq) < self.config.seq_len: if k - idx < 0: #this is transient, can only append zeros fname_seq.append(-1) fts_seq.append(0.) continue current_idx = k - idx rollout_idx = self.features[current_idx][0] if idx == 0: fname_seq.append(current_idx) fts_seq.append(kth_fts) rollout_idxes.append(rollout_idx) else: if rollout_idx != rollout_idxes[-1]: # it is a transient! Can only append zeros. fname_seq.append(-1) fts_seq.append(0.) else: # Check the features are different fname = self.filenames[current_idx] fts = np.load(fname, allow_pickle=True).item() if not self.check_equal_dict(fts, fts_seq[-1]): # Objects are not equal, can append fname_seq.append(current_idx) fts_seq.append(fts) rollout_idxes.append(rollout_idx) idx += 1 assert len(fts_seq) == len(fname_seq) self.stacked_filenames.append(fname_seq) # EndFor assert len(self.filenames) == len(self.stacked_filenames) def _build_dataset(self): # Need to take care that rollout_idxs are consistent self.features = np.stack(self.features) self.features = self.features.astype(np.float32) self.labels = np.stack(self.labels) self.labels = self.labels.astype(np.float32) last_fname_numbers = [] if self.config.use_fts_tracks: self._preprocess_fnames() # Preprocess filenames to assess consistency of experiment for idx in range(self.config.seq_len-1, self.samples): if self.features[idx,0] == self.features[idx-self.config.seq_len+1,0]: last_fname_numbers.append(np.int32(idx)) if self.training: np.random.shuffle(last_fname_numbers) # Form training batches dataset = tf.data.Dataset.from_tensor_slices(last_fname_numbers) if self.training: dataset = dataset.shuffle(buffer_size=len(last_fname_numbers)) dataset = dataset.map(self._dataset_map, num_parallel_calls=10 if self.training else 1) dataset = dataset.batch(self.config.batch_size, drop_remainder=not self.training) dataset = dataset.prefetch(buffer_size=10*self.config.batch_size) self.batched_dataset = dataset ================================================ FILE: controller_learning/src/ControllerLearning/models/bodyrate_learner.py ================================================ import datetime import os import numpy as np import tensorflow as tf from tqdm import tqdm # Required for catkin import strategy try: from .nets import create_network from .body_dataset import create_dataset except: from nets import create_network from body_dataset import create_dataset os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' class BodyrateLearner(object): def __init__(self, settings): self.config = settings physical_devices = tf.config.experimental.list_physical_devices('GPU') if len(physical_devices) > 0: tf.config.experimental.set_memory_growth(physical_devices[0], True) self.min_val_loss = tf.Variable(np.inf, name='min_val_loss', trainable=False) self.network = create_network(self.config) self.loss = tf.keras.losses.MeanSquaredError() self.optimizer = tf.keras.optimizers.Adam(learning_rate=1e-4, clipvalue=.2) self.train_loss = tf.keras.metrics.Mean(name='train_loss') self.val_loss = tf.keras.metrics.Mean(name='validation_loss') self.global_epoch = tf.Variable(0) self.ckpt = tf.train.Checkpoint(step=self.global_epoch, optimizer=self.optimizer, net=self.network) if self.config.resume_training: if self.ckpt.restore(self.config.resume_ckpt_file): print("------------------------------------------") print("Restored from {}".format(self.config.resume_ckpt_file)) print("------------------------------------------") return print("------------------------------------------") print("Initializing from scratch.") print("------------------------------------------") @tf.function def train_step(self, inputs, labels): with tf.GradientTape() as tape: predictions = self.network(inputs) loss = self.loss(labels, predictions) gradients = tape.gradient(loss, self.network.trainable_variables) self.optimizer.apply_gradients(zip(gradients, self.network.trainable_variables)) self.train_loss.update_state(loss) return gradients @tf.function def val_step(self, inputs, labels): predictions = self.network(inputs) loss = self.loss(labels, predictions) self.val_loss.update_state(loss) def adapt_input_data(self, features): if self.config.use_fts_tracks: inputs = {"fts": features[1], "state": features[0]} else: inputs = {"state": features} return inputs def write_train_summaries(self, features, gradients): with self.summary_writer.as_default(): tf.summary.scalar('Train Loss', self.train_loss.result(), step=self.optimizer.iterations) for g, v in zip(gradients, self.network.trainable_variables): tf.summary.histogram(v.name, g, step=self.optimizer.iterations) def train(self): print("Training Network") if not hasattr(self, 'train_log_dir'): # This should be done only once self.train_log_dir = os.path.join(self.config.log_dir, 'train') self.summary_writer = tf.summary.create_file_writer(self.train_log_dir) self.ckpt_manager = tf.train.CheckpointManager(self.ckpt, self.train_log_dir, max_to_keep=10) else: # We are in dagger mode, so let us reset the best loss self.min_val_loss = np.inf self.train_loss.reset_states() self.val_loss.reset_states() dataset_train = create_dataset(self.config.train_dir, self.config, training=True) dataset_val = create_dataset(self.config.val_dir, self.config, training=False) for epoch in range(self.config.max_training_epochs): # Train for k, (features, label) in enumerate(tqdm(dataset_train.batched_dataset)): features = self.adapt_input_data(features) gradients = self.train_step(features, label) if tf.equal(k % self.config.summary_freq, 0): self.write_train_summaries(features, gradients) self.train_loss.reset_states() # Eval for features, label in tqdm(dataset_val.batched_dataset): features = self.adapt_input_data(features) self.val_step(features, label) validation_loss = self.val_loss.result() with self.summary_writer.as_default(): tf.summary.scalar("Validation Loss", validation_loss, step=tf.cast(self.global_epoch, dtype=tf.int64)) self.val_loss.reset_states() self.global_epoch = self.global_epoch + 1 self.ckpt.step.assign_add(1) print("Epoch: {:2d}, Validation Loss: {:.4f}".format(self.global_epoch, validation_loss)) if validation_loss < self.min_val_loss or ((epoch + 1) % self.config.save_every_n_epochs) == 0: if validation_loss < self.min_val_loss: self.min_val_loss = validation_loss save_path = self.ckpt_manager.save() print("Saved checkpoint for epoch {}: {}".format(int(self.ckpt.step), save_path)) # Reset the metrics for the next epoch print("------------------------------") print("Training finished successfully") print("------------------------------") def test(self): print("Testing Network") self.train_log_dir = os.path.join(self.config.log_dir, 'test') dataset_val = create_dataset(self.config.test_dir, self.config, training=False) for features, label in tqdm(dataset_val.batched_dataset): features = self.adapt_input_data(features) self.val_step(features, label) validation_loss = self.val_loss.result() self.val_loss.reset_states() print("Testing Loss: {:.4f}".format(validation_loss)) @tf.function def inference(self, inputs): predictions = self.network(inputs) return predictions ================================================ FILE: controller_learning/src/ControllerLearning/models/nets.py ================================================ import tensorflow as tf from tensorflow.keras import Model from tensorflow.keras.layers import Dense, Conv2D, LeakyReLU, Conv1D from tensorflow.keras.layers import Flatten, GlobalAveragePooling2D try: from .tf_addons_normalizations import InstanceNormalization except: from tf_addons_normalizations import InstanceNormalization def create_network(settings): net = AggressiveNet(settings) return net class Network(Model): def __init__(self): super(Network, self).__init__() def create(self): self._create() def call(self, x): return self._internal_call(x) def _create(self): raise NotImplementedError def _internal_call(self): raise NotImplementedError class AggressiveNet(Network): def __init__(self, config): super(AggressiveNet, self).__init__() self.config = config self._create(input_size=(config.seq_len, config.min_number_fts, 5)) def _create(self, input_size, has_bias=True, learn_affine=True): """Init. Args: input_size (float): size of input has_bias (bool, optional): Defaults to True. Conv1d bias? learn_affine (bool, optional): Defaults to True. InstanceNorm1d affine? """ if self.config.use_fts_tracks: f = 2.0 self.pointnet = [Conv2D(int(16 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1, use_bias=has_bias, input_shape=input_size), InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine), LeakyReLU(alpha=1e-2), Conv2D(int(32 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1, use_bias=has_bias), InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine), LeakyReLU(alpha=1e-2), Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1, use_bias=has_bias), InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine), LeakyReLU(alpha=1e-2), Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1, use_bias=has_bias), GlobalAveragePooling2D()] input_size = (self.config.seq_len, int(64*f)) self.fts_mergenet = [Conv1D(int(64 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1, input_shape=input_size), LeakyReLU(alpha=1e-2), Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Flatten(), Dense(int(64*f))] g = 2.0 self.states_conv = [Conv1D(int(64 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1), LeakyReLU(alpha=1e-2), Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1), Flatten(), Dense(int(64*g))] self.control_module = [Dense(64*g), LeakyReLU(alpha=1e-2), Dense(32*g), LeakyReLU(alpha=1e-2), Dense(16*g), LeakyReLU(alpha=1e-2), Dense(4)] def _pointnet_branch(self, single_t_features): x = tf.expand_dims(single_t_features, axis=1) for f in self.pointnet: x = f(x) return x def _features_branch(self, input_features): preprocessed_fts = tf.map_fn(self._pointnet_branch, elems=input_features, parallel_iterations=self.config.seq_len) # (seq_len, batch_size, 64) preprocessed_fts = tf.transpose(preprocessed_fts, (1,0,2)) # (batch_size, seq_len, 64) x = preprocessed_fts for f in self.fts_mergenet: x = f(x) return x def _states_branch(self, embeddings): x = embeddings for f in self.states_conv: x = f(x) return x def _control_branch(self, embeddings): x = embeddings for f in self.control_module: x = f(x) return x def _internal_call(self, inputs): states = inputs['state'] if self.config.use_fts_tracks: fts_stack = inputs['fts'] # (batch_size, seq_len, min_numb_features, 5) fts_stack = tf.transpose(fts_stack, (1,0,2,3)) # (seq_len, batch_size, min_numb_features, 5) # Execute PointNet Part fts_embeddings = self._features_branch(fts_stack) states_embeddings = self._states_branch(states) if self.config.use_fts_tracks: total_embeddings = tf.concat((fts_embeddings, states_embeddings), axis=1) else: total_embeddings = states_embeddings output = self._control_branch(total_embeddings) return output ================================================ FILE: controller_learning/src/ControllerLearning/models/tf_addons_normalizations.py ================================================ # Copyright 2019 The TensorFlow Authors. All Rights Reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # Orginal implementation from keras_contrib/layer/normalization # The Authors of the drone_acrobatics repository do not own any right on this file. # If you have problems with this file, install the tf_addons repository directly. # ============================================================================= from __future__ import absolute_import from __future__ import division from __future__ import print_function import tensorflow as tf class GroupNormalization(tf.keras.layers.Layer): """Group normalization layer. Group Normalization divides the channels into groups and computes within each group the mean and variance for normalization. Empirically, its accuracy is more stable than batch norm in a wide range of small batch sizes, if learning rate is adjusted linearly with batch sizes. Relation to Layer Normalization: If the number of groups is set to 1, then this operation becomes identical to Layer Normalization. Relation to Instance Normalization: If the number of groups is set to the input dimension (number of groups is equal to number of channels), then this operation becomes identical to Instance Normalization. Arguments groups: Integer, the number of groups for Group Normalization. Can be in the range [1, N] where N is the input dimension. The input dimension must be divisible by the number of groups. axis: Integer, the axis that should be normalized. epsilon: Small float added to variance to avoid dividing by zero. center: If True, add offset of `beta` to normalized tensor. If False, `beta` is ignored. scale: If True, multiply by `gamma`. If False, `gamma` is not used. beta_initializer: Initializer for the beta weight. gamma_initializer: Initializer for the gamma weight. beta_regularizer: Optional regularizer for the beta weight. gamma_regularizer: Optional regularizer for the gamma weight. beta_constraint: Optional constraint for the beta weight. gamma_constraint: Optional constraint for the gamma weight. Input shape Arbitrary. Use the keyword argument `input_shape` (tuple of integers, does not include the samples axis) when using this layer as the first layer in a model. Output shape Same shape as input. References - [Group Normalization](https://arxiv.org/abs/1803.08494) """ def __init__(self, groups=2, axis=-1, epsilon=1e-3, center=True, scale=True, beta_initializer='zeros', gamma_initializer='ones', beta_regularizer=None, gamma_regularizer=None, beta_constraint=None, gamma_constraint=None, **kwargs): super(GroupNormalization, self).__init__(**kwargs) self.supports_masking = True self.groups = groups self.axis = axis self.epsilon = epsilon self.center = center self.scale = scale self.beta_initializer = tf.keras.initializers.get(beta_initializer) self.gamma_initializer = tf.keras.initializers.get(gamma_initializer) self.beta_regularizer = tf.keras.regularizers.get(beta_regularizer) self.gamma_regularizer = tf.keras.regularizers.get(gamma_regularizer) self.beta_constraint = tf.keras.constraints.get(beta_constraint) self.gamma_constraint = tf.keras.constraints.get(gamma_constraint) self._check_axis() def build(self, input_shape): self._check_if_input_shape_is_none(input_shape) self._set_number_of_groups_for_instance_norm(input_shape) self._check_size_of_dimensions(input_shape) self._create_input_spec(input_shape) self._add_gamma_weight(input_shape) self._add_beta_weight(input_shape) self.built = True super(GroupNormalization, self).build(input_shape) def call(self, inputs): input_shape = tf.keras.backend.int_shape(inputs) tensor_input_shape = tf.shape(inputs) reshaped_inputs, group_shape = self._reshape_into_groups( inputs, input_shape, tensor_input_shape) normalized_inputs = self._apply_normalization(reshaped_inputs, input_shape) outputs = tf.reshape(normalized_inputs, tensor_input_shape) return outputs def get_config(self): config = { 'groups': self.groups, 'axis': self.axis, 'epsilon': self.epsilon, 'center': self.center, 'scale': self.scale, 'beta_initializer': tf.keras.initializers.serialize(self.beta_initializer), 'gamma_initializer': tf.keras.initializers.serialize(self.gamma_initializer), 'beta_regularizer': tf.keras.regularizers.serialize(self.beta_regularizer), 'gamma_regularizer': tf.keras.regularizers.serialize(self.gamma_regularizer), 'beta_constraint': tf.keras.constraints.serialize(self.beta_constraint), 'gamma_constraint': tf.keras.constraints.serialize(self.gamma_constraint) } base_config = super(GroupNormalization, self).get_config() return dict(list(base_config.items()) + list(config.items())) def compute_output_shape(self, input_shape): return input_shape def _reshape_into_groups(self, inputs, input_shape, tensor_input_shape): group_shape = [tensor_input_shape[i] for i in range(len(input_shape))] group_shape[self.axis] = input_shape[self.axis] // self.groups group_shape.insert(1, self.groups) group_shape = tf.stack(group_shape) reshaped_inputs = tf.reshape(inputs, group_shape) return reshaped_inputs, group_shape def _apply_normalization(self, reshaped_inputs, input_shape): group_shape = tf.keras.backend.int_shape(reshaped_inputs) group_reduction_axes = list(range(len(group_shape))) # Remember the ordering of the tensor is [batch, group , steps]. Jump # the first 2 to calculate the variance and the mean mean, variance = tf.nn.moments( reshaped_inputs, group_reduction_axes[2:], keepdims=True) gamma, beta = self._get_reshaped_weights(input_shape) normalized_inputs = tf.nn.batch_normalization( reshaped_inputs, mean=mean, variance=variance, scale=gamma, offset=beta, variance_epsilon=self.epsilon) return normalized_inputs def _get_reshaped_weights(self, input_shape): broadcast_shape = self._create_broadcast_shape(input_shape) gamma = None beta = None if self.scale: gamma = tf.reshape(self.gamma, broadcast_shape) if self.center: beta = tf.reshape(self.beta, broadcast_shape) return gamma, beta def _check_if_input_shape_is_none(self, input_shape): dim = input_shape[self.axis] if dim is None: raise ValueError('Axis ' + str(self.axis) + ' of ' 'input tensor should have a defined dimension ' 'but the layer received an input with shape ' + str(input_shape) + '.') def _set_number_of_groups_for_instance_norm(self, input_shape): dim = input_shape[self.axis] if self.groups == -1: self.groups = dim def _check_size_of_dimensions(self, input_shape): dim = input_shape[self.axis] if dim < self.groups: raise ValueError( 'Number of groups (' + str(self.groups) + ') cannot be ' 'more than the number of channels (' + str(dim) + ').') if dim % self.groups != 0: raise ValueError( 'Number of groups (' + str(self.groups) + ') must be a ' 'multiple of the number of channels (' + str(dim) + ').') def _check_axis(self): if self.axis == 0: raise ValueError( "You are trying to normalize your batch axis. Do you want to " "use tf.layer.batch_normalization instead") def _create_input_spec(self, input_shape): dim = input_shape[self.axis] self.input_spec = tf.keras.layers.InputSpec( ndim=len(input_shape), axes={self.axis: dim}) def _add_gamma_weight(self, input_shape): dim = input_shape[self.axis] shape = (dim,) if self.scale: self.gamma = self.add_weight( shape=shape, name='gamma', initializer=self.gamma_initializer, regularizer=self.gamma_regularizer, constraint=self.gamma_constraint) else: self.gamma = None def _add_beta_weight(self, input_shape): dim = input_shape[self.axis] shape = (dim,) if self.center: self.beta = self.add_weight( shape=shape, name='beta', initializer=self.beta_initializer, regularizer=self.beta_regularizer, constraint=self.beta_constraint) else: self.beta = None def _create_broadcast_shape(self, input_shape): broadcast_shape = [1] * len(input_shape) broadcast_shape[self.axis] = input_shape[self.axis] // self.groups broadcast_shape.insert(1, self.groups) return broadcast_shape # @keras_utils.register_keras_custom_object class InstanceNormalization(GroupNormalization): """Instance normalization layer. Instance Normalization is an specific case of ```GroupNormalization```since it normalizes all features of one channel. The Groupsize is equal to the channel size. Empirically, its accuracy is more stable than batch norm in a wide range of small batch sizes, if learning rate is adjusted linearly with batch sizes. Arguments axis: Integer, the axis that should be normalized. epsilon: Small float added to variance to avoid dividing by zero. center: If True, add offset of `beta` to normalized tensor. If False, `beta` is ignored. scale: If True, multiply by `gamma`. If False, `gamma` is not used. beta_initializer: Initializer for the beta weight. gamma_initializer: Initializer for the gamma weight. beta_regularizer: Optional regularizer for the beta weight. gamma_regularizer: Optional regularizer for the gamma weight. beta_constraint: Optional constraint for the beta weight. gamma_constraint: Optional constraint for the gamma weight. Input shape Arbitrary. Use the keyword argument `input_shape` (tuple of integers, does not include the samples axis) when using this layer as the first layer in a model. Output shape Same shape as input. References - [Instance Normalization: The Missing Ingredient for Fast Stylization] (https://arxiv.org/abs/1607.08022) """ def __init__(self, **kwargs): kwargs["groups"] = -1 super(InstanceNormalization, self).__init__(**kwargs) ================================================ FILE: controller_learning/test_trajectories.py ================================================ import argparse from common import setup_sim, update_mpc_params from config.settings import create_settings from iterative_learning_trajectories import Trainer def main(): parser = argparse.ArgumentParser(description='Evaluate Trajectory tracker.') parser.add_argument('--settings_file', help='Path to settings yaml', required=True) args = parser.parse_args() settings_filepath = args.settings_file settings = create_settings(settings_filepath, mode='test') update_mpc_params() setup_sim() trainer = Trainer(settings) trainer.perform_testing() if __name__ == "__main__": main() ================================================ FILE: controller_learning/train.py ================================================ #!/usr/bin/env python3 import argparse import os import sys sys.path.append("./src/ControllerLearning/models/") import time #import rospy from bodyrate_learner import BodyrateLearner from config.settings import create_settings def main(): parser = argparse.ArgumentParser(description='Train Network') parser.add_argument('--settings_file', help='Path to settings yaml', required=True) args = parser.parse_args() settings_filepath = args.settings_file settings = create_settings(settings_filepath, mode='train') learner = BodyrateLearner(settings=settings) learner.train() if __name__ == "__main__": main() ================================================ FILE: controller_learning/train_logs/20191015-164750/train/checkpoint ================================================ model_checkpoint_path: "ckpt-80" all_model_checkpoint_paths: "ckpt-78" all_model_checkpoint_paths: "ckpt-79" all_model_checkpoint_paths: "ckpt-80" all_model_checkpoint_timestamps: 1571151095.56399 all_model_checkpoint_timestamps: 1571151096.0123613 all_model_checkpoint_timestamps: 1571151096.455976 last_preserved_timestamp: 1571150869.450073 ================================================ FILE: controller_learning/train_logs/20191022-082718/train/checkpoint ================================================ model_checkpoint_path: "ckpt-1080" all_model_checkpoint_paths: "ckpt-1078" all_model_checkpoint_paths: "ckpt-1079" all_model_checkpoint_paths: "ckpt-1080" all_model_checkpoint_timestamps: 1571726594.0011396 all_model_checkpoint_timestamps: 1571726594.9023852 all_model_checkpoint_timestamps: 1571726595.8179815 last_preserved_timestamp: 1571725637.457743 ================================================ FILE: controller_learning/train_logs/20191022-095948/train/checkpoint ================================================ model_checkpoint_path: "ckpt-1000" all_model_checkpoint_paths: "ckpt-998" all_model_checkpoint_paths: "ckpt-999" all_model_checkpoint_paths: "ckpt-1000" all_model_checkpoint_timestamps: 1571733868.2468255 all_model_checkpoint_timestamps: 1571733870.8290527 all_model_checkpoint_timestamps: 1571733873.409515 last_preserved_timestamp: 1571731187.6806133 ================================================ FILE: custom_rotors_interface/.gitignore ================================================ cmake-build-debug/ ================================================ FILE: custom_rotors_interface/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(custom_rotors_interface) # Add plain cmake packages find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) cs_add_library(custom_rotors_interface src/custom_rotors_interface.cpp) cs_add_executable(custom_rotors_interface_node src/custom_rotors_interface_node.cpp) target_link_libraries(custom_rotors_interface_node custom_rotors_interface) cs_install() cs_export() # CMake Indexing FILE(GLOB_RECURSE LibFiles "include/*") add_custom_target(headers SOURCES ${LibFiles}) FILE(GLOB_RECURSE ProtoFiles "proto/*") add_custom_target(protos SOURCES ${ProtoFiles}) ================================================ FILE: custom_rotors_interface/include/custom_rotors_interface/custom_rotors_interface.h ================================================ #pragma once #include #include #include #include #include #include "mav_msgs/Actuators.h" #include "nav_msgs/Odometry.h" #include "quadrotor_msgs/AutopilotFeedback.h" #include "pose_utils/pose.h" #include "std_msgs/Bool.h" namespace custom_rotors_interface { struct TorquesAndThrust { Eigen::Vector3d body_torques; double collective_thrust; }; class CustomRotorsInterface { public: CustomRotorsInterface(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); CustomRotorsInterface() : CustomRotorsInterface(ros::NodeHandle(), ros::NodeHandle("~")) {} private: void armCallback(const std_msgs::BoolConstPtr& msg); void controlCommandCallback( const quadrotor_msgs::ControlCommandConstPtr& msg); void odometryCallback(const nav_msgs::OdometryConstPtr& msg); void autopilotCallback(const quadrotor_msgs::AutopilotFeedbackConstPtr& msg); void motorSpeedCallback(const mav_msgs::Actuators::ConstPtr& msg); void lowLevelControlLoop(); TorquesAndThrust bodyRateControl( const quadrotor_common::ControlCommand& rate_cmd, const Eigen::Vector3d& body_rate_estimate); mav_msgs::Actuators mixer(const TorquesAndThrust& torques_and_thrust); bool loadParameters(); ros::NodeHandle nh_; ros::NodeHandle pnh_; ros::Subscriber arm_sub_; ros::Subscriber motor_speed_sub_; ros::Subscriber ctrl_cmd_sub_; ros::Subscriber imu_sub_; ros::Subscriber odometry_sub_; ros::Subscriber controller_sub_; ros::Subscriber network_sub_; ros::Subscriber autopilot_sub_; ros::Publisher desired_motor_speed_pub_; ros::WallTime start_time_; std::string data_dir_; Eigen::Vector3d prev_v_lin_, prev_v_ang_; Eigen::Vector3d incr_v_lin_, incr_v_ang_; ros::Time last_hover_time_ = ros::Time::now(); int maneuver_idx_ = 0; int action_idx_ = 0; bool velocity_estimate_in_world_frame_; bool enable_navigation_ = false; bool reference_frame_set_ = false; bool interface_armed_ = false; TorquesAndThrust torques_and_thrust_estimate_; double inertia_x_; double inertia_y_; double inertia_z_; double body_rates_p_xy_; double body_rates_d_xy_; double body_rates_p_z_; double body_rates_d_z_; // Parameters Eigen::Matrix3d inertia_; Eigen::MatrixXd K_lqr_; double low_level_control_frequency_; double roll_pitch_cont_gain_; double arm_length_; double rotor_drag_coeff_; double rotor_thrust_coeff_; double mass_; double max_rotor_speed_; quadrotor_msgs::ControlCommand control_command_; nav_msgs::Odometry odometry_; quadrotor_msgs::AutopilotFeedback autopilot_feedback_; double rel_time_; }; } // namespace custom_rotors_interface ================================================ FILE: custom_rotors_interface/package.xml ================================================ custom_rotors_interface 0.0.0 A package containing utils for simulation Elia Kaufmann GNU GPL catkin catkin_simple roscpp gazebo gazebo_msgs mav_msgs quadrotor_common pose_utils rotors_gazebo rotors_description manual_flight_assistant rqt_quad_gui ================================================ FILE: custom_rotors_interface/parameters/autopilot.yaml ================================================ state_estimate_timeout: 5.0 # [s] velocity_estimate_in_world_frame: false control_command_delay: 0.05 # [s] start_land_velocity: 0.5 # [m/s] start_land_acceleration: 1 # [m/s^2] start_idle_duration: 2.0 # [s] idle_thrust: 2.0 # [m/s] optitrack_start_height: 3.0 # [m] optitrack_start_land_timeout: 10 # [s] optitrack_land_drop_height: 0.3 # [m] propeller_ramp_down_timeout: 1.5 # [s] breaking_velocity_threshold: 0.2 # [m/s] breaking_timeout: 0.5 # [s] go_to_pose_max_velocity: 3.5 # [m/s] go_to_pose_max_normalized_thrust: 15.0 # [m/s^2] go_to_pose_max_roll_pitch_rate: 1.0 # [rad/s] velocity_command_input_timeout: 0.1 # [s] tau_velocity_command: 0.8 # [] reference_state_input_timeout: 0.1 # [s] emergency_land_duration: 4 # [s] emergency_land_thrust: 9.0 # [m/s^2] control_command_input_timeout: 1.0 # [s] enable_command_feedthrough: true predictive_control_lookahead: 2.0 # [s] min_control_period: 0.0 # [s] ================================================ FILE: custom_rotors_interface/parameters/custom_rotors_interface.yaml ================================================ # Vehicle parameters inertia_x: 0.007 inertia_y: 0.007 inertia_z: 0.012 arm_length: 0.17 mass: 0.73 # Motors parameters rotor_drag_coeff: 0.016 rotor_thrust_coeff: 8.54858e-06 max_rotor_speed: 838.0 # Controller low_level_control_frequency: 200.0 body_rates_p_xy: 0.1 body_rates_d_xy: 0.5 body_rates_p_z: 0.03 body_rates_d_z: 0.1 roll_pitch_cont_gain: 6.0 ================================================ FILE: custom_rotors_interface/parameters/manual_flight_assistant.yaml ================================================ joypad_timeout: 0.5 sbus_timeout: 0.5 joypad_axes_zero_tolerance: 0.05 sbus_axes_zero_tolerance: 10 vmax_xy: 1.5 vmax_z: 0.7 rmax_yaw: 1.5 ================================================ FILE: custom_rotors_interface/parameters/position_controller.yaml ================================================ position_controller: use_rate_mode: true kpxy: 10.0 kdxy: 4.0 kpz: 15.0 kdz: 6.0 krp: 12.0 kyaw: 5.0 pxy_error_max: 0.6 vxy_error_max: 1.0 pz_error_max: 0.3 vz_error_max: 0.75 yaw_error_max: 0.7 perform_aerodynamics_compensation: false k_drag_x: 0.0 k_drag_y: 0.0 k_drag_z: 0.0 k_thrust_horz: 0.0 ================================================ FILE: custom_rotors_interface/src/custom_rotors_interface.cpp ================================================ #include "custom_rotors_interface/custom_rotors_interface.h" #include #include "quadrotor_common/geometry_eigen_conversions.h" namespace custom_rotors_interface { CustomRotorsInterface::CustomRotorsInterface(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) : nh_(nh), pnh_(pnh) { if (!loadParameters()) { ROS_ERROR("[%s] Could not load parameters.", pnh_.getNamespace().c_str()); } arm_sub_ = nh_.subscribe("bridge/arm", 1, &CustomRotorsInterface::armCallback, this); ctrl_cmd_sub_ = nh_.subscribe("control_command", 1, &CustomRotorsInterface::controlCommandCallback, this, ros::TransportHints().tcpNoDelay()); autopilot_sub_ = nh_.subscribe("autopilot/feedback", 1, &CustomRotorsInterface::autopilotCallback, this, ros::TransportHints().tcpNoDelay()); odometry_sub_ = nh_.subscribe("odometry", 1, &CustomRotorsInterface::odometryCallback, this, ros::TransportHints().tcpNoDelay()); motor_speed_sub_ = nh_.subscribe("motor_speed", 1, &CustomRotorsInterface::motorSpeedCallback, this, ros::TransportHints().tcpNoDelay()); desired_motor_speed_pub_ = nh_.advertise("command/motor_speed", 1); } void CustomRotorsInterface::armCallback(const std_msgs::BoolConstPtr& msg) { if (msg->data) { interface_armed_ = true; ROS_INFO("[%s] Interface armed", pnh_.getNamespace().c_str()); } else { interface_armed_ = false; ROS_INFO("[%s] Interface disarmed", pnh_.getNamespace().c_str()); } } void CustomRotorsInterface::controlCommandCallback( const quadrotor_msgs::ControlCommandConstPtr& msg) { control_command_ = *msg; } void CustomRotorsInterface::autopilotCallback( const quadrotor_msgs::AutopilotFeedbackConstPtr& msg) { autopilot_feedback_ = *msg; } void CustomRotorsInterface::odometryCallback( const nav_msgs::OdometryConstPtr& msg) { odometry_ = *msg; } void CustomRotorsInterface::motorSpeedCallback( const mav_msgs::Actuators::ConstPtr& msg) { // The hummingbird that we use in our simulations has the following rotor // configuration Rotor 0 spins clockwise // x // 0 ^ // | | // 1--+--3 y <--+ // | // 2 const double f0 = rotor_thrust_coeff_ * pow(msg->angular_velocities[0], 2.0); const double f1 = rotor_thrust_coeff_ * pow(msg->angular_velocities[1], 2.0); const double f2 = rotor_thrust_coeff_ * pow(msg->angular_velocities[2], 2.0); const double f3 = rotor_thrust_coeff_ * pow(msg->angular_velocities[3], 2.0); torques_and_thrust_estimate_.body_torques.x() = arm_length_ * (f1 - f3); torques_and_thrust_estimate_.body_torques.y() = arm_length_ * (f2 - f0); torques_and_thrust_estimate_.body_torques.z() = rotor_drag_coeff_ * (f0 - f1 + f2 - f3); torques_and_thrust_estimate_.collective_thrust = f0 + f1 + f2 + f3; lowLevelControlLoop(); } void CustomRotorsInterface::lowLevelControlLoop() { mav_msgs::Actuators desired_motor_speed; quadrotor_msgs::ControlCommand control_command = control_command_; nav_msgs::Odometry quad_state = odometry_; if (!interface_armed_ || !control_command.armed) { for (int i = 0; i < 4; i++) { desired_motor_speed.angular_velocities.push_back(0.0); } } else { if (control_command.control_mode == control_command.BODY_RATES) { const quadrotor_common::ControlCommand rate_cmd = quadrotor_common::ControlCommand(control_command); const TorquesAndThrust torques_and_thrust = bodyRateControl( rate_cmd, quadrotor_common::geometryToEigen(quad_state.twist.twist.angular)); desired_motor_speed = mixer(torques_and_thrust); } else { ROS_ERROR_THROTTLE(1, "[%s] Undefined control mode, will not apply command.", ros::this_node::getName().c_str()); return; } } desired_motor_speed_pub_.publish(desired_motor_speed); } TorquesAndThrust CustomRotorsInterface::bodyRateControl( const quadrotor_common::ControlCommand& rate_cmd, const Eigen::Vector3d& body_rate_estimate) { Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 3); Eigen::VectorXd control_error = Eigen::VectorXd::Zero(6); // control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate; // control_error.segment(3, 3) = rate_cmd.bodyrates.cross(inertia_ * // rate_cmd.bodyrates) // + inertia_ * rate_cmd.angular_accelerations // - torques_and_thrust_estimate_.body_torques; control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate; control_error.segment(3, 3) = rate_cmd.bodyrates.cross(inertia_ * rate_cmd.bodyrates) + inertia_ * rate_cmd.angular_accelerations - torques_and_thrust_estimate_.body_torques; TorquesAndThrust torques_and_thrust; // torques_and_thrust.body_torques = K_lqr_ * control_error + // body_rate_estimate.cross(inertia_ * body_rate_estimate) // + inertia_ * // rate_cmd.angular_accelerations; // new version torques_and_thrust.body_torques = K_lqr_ * control_error + body_rate_estimate.cross(inertia_ * body_rate_estimate) + inertia_ * rate_cmd.angular_accelerations; torques_and_thrust.collective_thrust = rate_cmd.collective_thrust; return torques_and_thrust; } mav_msgs::Actuators CustomRotorsInterface::mixer( const TorquesAndThrust& torques_and_thrust) { // Using Rotor Convention of the Hummingbird mav_msgs::Actuators rotor_speed_cmds; for (int i = 0; i < 4; i++) { rotor_speed_cmds.angular_velocities.push_back(0.0); } // Compute square of single rotor speed commands if (torques_and_thrust.collective_thrust < 0.05) { return rotor_speed_cmds; } // Compute the square of the single rotor speeds rotor_speed_cmds.angular_velocities[0] = ((arm_length_ * torques_and_thrust.body_torques.z() - 2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() + rotor_drag_coeff_ * arm_length_ * mass_ * torques_and_thrust.collective_thrust) / (4.0 * rotor_drag_coeff_ * arm_length_)) / rotor_thrust_coeff_; rotor_speed_cmds.angular_velocities[1] = ((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() - arm_length_ * torques_and_thrust.body_torques.z() + rotor_drag_coeff_ * arm_length_ * mass_ * torques_and_thrust.collective_thrust) / (4.0 * rotor_drag_coeff_ * arm_length_)) / rotor_thrust_coeff_; rotor_speed_cmds.angular_velocities[2] = ((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() + arm_length_ * torques_and_thrust.body_torques.z() + rotor_drag_coeff_ * arm_length_ * mass_ * torques_and_thrust.collective_thrust) / (4.0 * rotor_drag_coeff_ * arm_length_)) / rotor_thrust_coeff_; rotor_speed_cmds.angular_velocities[3] = (-(2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() + arm_length_ * torques_and_thrust.body_torques.z() - rotor_drag_coeff_ * arm_length_ * mass_ * torques_and_thrust.collective_thrust) / (4.0 * rotor_drag_coeff_ * arm_length_)) / rotor_thrust_coeff_; // Apply limits and take square root for (int i = 0; i < 4; i++) { quadrotor_common::limit(&rotor_speed_cmds.angular_velocities[i], 0.0, pow(max_rotor_speed_, 2.0)); rotor_speed_cmds.angular_velocities[i] = sqrt(rotor_speed_cmds.angular_velocities[i]); } rotor_speed_cmds.header.stamp = ros::Time::now(); return rotor_speed_cmds; } bool CustomRotorsInterface::loadParameters() { bool check = true; check &= pnh_.getParam("inertia_x", inertia_x_); check &= pnh_.getParam("inertia_y", inertia_y_); check &= pnh_.getParam("inertia_z", inertia_z_); inertia_ = Eigen::Matrix3d::Zero(); inertia_(0, 0) = inertia_x_; inertia_(1, 1) = inertia_y_; inertia_(2, 2) = inertia_z_; check &= pnh_.getParam("body_rates_p_xy", body_rates_p_xy_); check &= pnh_.getParam("body_rates_d_xy", body_rates_d_xy_); check &= pnh_.getParam("body_rates_p_z", body_rates_p_z_); check &= pnh_.getParam("body_rates_d_z", body_rates_d_z_); // nothing to do with lqr, just a PD controller K_lqr_ = Eigen::MatrixXd::Zero(3, 6); K_lqr_(0, 0) = body_rates_p_xy_; K_lqr_(1, 1) = body_rates_p_xy_; K_lqr_(2, 2) = body_rates_p_z_; K_lqr_(0, 3) = body_rates_d_xy_; K_lqr_(1, 4) = body_rates_d_xy_; K_lqr_(2, 5) = body_rates_d_z_; check &= pnh_.getParam("roll_pitch_cont_gain", roll_pitch_cont_gain_); check &= pnh_.getParam("arm_length", arm_length_); check &= pnh_.getParam("rotor_drag_coeff", rotor_drag_coeff_); check &= pnh_.getParam("rotor_thrust_coeff", rotor_thrust_coeff_); check &= pnh_.getParam("mass", mass_); check &= pnh_.getParam("max_rotor_speed", max_rotor_speed_); return check; } } // namespace custom_rotors_interface ================================================ FILE: custom_rotors_interface/src/custom_rotors_interface_node.cpp ================================================ #include #include int main(int argc, char **argv) { ros::init(argc, argv, "custom_rotors_interface"); custom_rotors_interface::CustomRotorsInterface custom_rotors_interface; ros::spin(); return 0; } ================================================ FILE: dependencies.yaml ================================================ repositories: catkin_boost_python_buildtool: type: git url: git@github.com:ethz-asl/catkin_boost_python_buildtool.git version: master catkin_simple: type: git url: git@github.com:catkin/catkin_simple.git version: master eigen_catkin: type: git url: git@github.com:ethz-asl/eigen_catkin.git version: master eigen_checks: type: git url: git@github.com:ethz-asl/eigen_checks.git version: master gflags_catkin: type: git url: git@github.com:ethz-asl/gflags_catkin.git version: master glog_catkin: type: git url: git@github.com:ethz-asl/glog_catkin.git version: master minkindr: type: git url: git@github.com:ethz-asl/minkindr.git version: master minkindr_ros: type: git url: git@github.com:ethz-asl/minkindr_ros.git version: master numpy_eigen: type: git url: git@github.com:ethz-asl/numpy_eigen.git version: master pose_utils: type: git url: git@github.com:antonilo/pose_utils.git version: master minimum_jerk_trajectories: type: git url: git@github.com:uzh-rpg/minimum_jerk_trajectories.git version: master octomap: type: git url: git@github.com:OctoMap/octomap.git version: master octomap_msgs: type: git url: git@github.com:OctoMap/octomap_msgs.git version: master octomap_ros: type: git url: git@github.com:OctoMap/octomap_ros.git version: melodic-devel rpg_mpc: type: git url: git@github.com:uzh-rpg/rpg_mpc.git version: master rotors_simulator: type: git url: git@github.com:ethz-asl/rotors_simulator.git version: master rpg_quadrotor_control: type: git url: git@github.com:uzh-rpg/rpg_quadrotor_control.git version: master rpg_quadrotor_common: type: git url: git@github.com:uzh-rpg/rpg_quadrotor_common.git version: master mav_comm: type: git url: git@github.com:ethz-asl/mav_comm.git version: master VINS-Mono: type: git url: git@github.com:kelia/VINS-Mono.git version: imu_constant_integration ================================================ FILE: fpv_aggressive_trajectories/.gitignore ================================================ cmake-build-debug/ .idea/ ================================================ FILE: fpv_aggressive_trajectories/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(fpv_aggressive_trajectories) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) add_compile_options(-O3) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) cs_add_library(visualize src/visualize.cpp) cs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp) target_include_directories(fpv_aggressive_trajectories PUBLIC ../../rpg_mpc/model/quadrotor_mpc_codegen/ ../../rpg_mpc/externals/qpoases ../../rpg_mpc/externals/qpoases/INCLUDE ../../rpg_mpc/externals/qpoases/SRC) target_link_libraries(fpv_aggressive_trajectories visualize) cs_add_executable(odometry_republisher src/odometry_republisher.cpp) cs_install() cs_export() ================================================ FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/fpv_aggressive_trajectories.h ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include "fpv_aggressive_trajectories/visualize.h" namespace fpv_aggressive_trajectories { class FPVAggressiveTrajectories { public: FPVAggressiveTrajectories(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); FPVAggressiveTrajectories() : FPVAggressiveTrajectories(ros::NodeHandle(), ros::NodeHandle("~")) {} virtual ~FPVAggressiveTrajectories(); private: enum STATES { kOff = 0, kComputeTrajectory = 1, kExecuteTrajectory = 2, kNetworkControl = 3, kAutopilot = 4 }; ros::NodeHandle nh_; ros::NodeHandle pnh_; // Subscribers ros::Subscriber toggle_experiment_sub_; ros::Subscriber odometry_sub_; ros::Subscriber odometry_gt_sub_; // Publishers ros::Publisher control_command_pub_; ros::Publisher control_command_gt_pub_; ros::Publisher aggressive_end_pub_; ros::Publisher marker_pub_ref_; ros::Publisher vio_ref_pub_; ros::Publisher traj_comp_finish_pub_; quadrotor_common::QuadStateEstimate getPredictedStateEstimate( const ros::Time& time, const state_predictor::StatePredictor* predictor); void computeManeuver(); quadrotor_common::ControlCommand computeControlCommand( const quadrotor_common::QuadStateEstimate& state_estimate, ros::Duration* trajectory_execution_left_duration, int* trajectories_left_in_queue); void publishControlCommand( const quadrotor_common::ControlCommand& control_command, const quadrotor_common::ControlCommand& control_command_gt); void startExecutionCallback(const std_msgs::BoolConstPtr& msg); void odometryCallback(const nav_msgs::OdometryConstPtr& msg); void odometryGTCallback(const nav_msgs::OdometryConstPtr& msg); bool loadParameters(); std::list trajectory_queue_; // will be the same trajectories, but transformed to GT frame std::list trajectory_queue_gt_; // Parameters double desired_heading_; double exec_loop_rate_; double circle_velocity_; int n_loops_; nav_msgs::Odometry odometry_gt_; STATES state_machine_{kComputeTrajectory}; ros::Time time_start_trajectory_execution_; quadrotor_common::QuadStateEstimate received_state_est_; quadrotor_common::QuadStateEstimate received_state_est_gt_; bool first_time_in_new_state_; double predictive_control_lookahead_ = 2.0; bool sent_maneuver_end_msg_ = false; int maneuver_counter_ = 0; double traj_sampling_freq_ = 50.0; // MPC controller variant rpg_mpc::MpcController base_controller_ = rpg_mpc::MpcController(ros::NodeHandle(), ros::NodeHandle("~"), "vio_mpc_path"); rpg_mpc::MpcParams base_controller_params_; state_predictor::StatePredictor state_predictor_; // Second controller instance that operates on ground truth data rpg_mpc::MpcParams base_controller_params_gt_; state_predictor::StatePredictor state_predictor_gt_; std::shared_ptr visualizer_; }; } // namespace fpv_aggressive_trajectories ================================================ FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/odometry_republisher.h ================================================ #pragma once #include #include #include namespace odometry_republisher { class OdometryRepublisher { public: OdometryRepublisher(); virtual ~OdometryRepublisher(); private: ros::NodeHandle nh_; ros::Subscriber state_estimate_sub_; ros::Publisher odometry_out_pub_; rpg::Pose T_B_S_; void odometryInCallback(const nav_msgs::OdometryConstPtr& msg); void transformOdometry(const nav_msgs::OdometryConstPtr& odom_in, nav_msgs::Odometry* odom_out); }; } // namespace odometry_republisher ================================================ FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/visualize.h ================================================ #pragma once #include #include #include "quadrotor_common/trajectory.h" namespace fpv_aggressive_trajectories { class Visualizer { public: Visualizer(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); Visualizer() : Visualizer(ros::NodeHandle(), ros::NodeHandle("~")) {} virtual ~Visualizer(); void visualizeReferenceTrajectory( const quadrotor_common::Trajectory* trajectory); void visualizeTrajectories( const std::list& maneuver_list); void create_vehicle_markers(int num_rotors, float arm_len, float body_width, float body_height); void displayQuadrotor(); private: ros::NodeHandle nh_; ros::NodeHandle pnh_; ros::Publisher bodyrates_viz_pub_; ros::Publisher marker_ref_pub_; ros::Publisher marker_pub_; ros::Publisher vehicle_marker_pub_; std::shared_ptr vehicle_marker_; }; } // namespace fpv_aggressive_trajectories ================================================ FILE: fpv_aggressive_trajectories/launch/simulation/controller.launch ================================================ ================================================ FILE: fpv_aggressive_trajectories/launch/simulation/custom_rotors_interface.launch ================================================ ================================================ FILE: fpv_aggressive_trajectories/launch/simulation/rpg_mpc.launch ================================================ ================================================ FILE: fpv_aggressive_trajectories/launch/simulation/rpg_pid.launch ================================================ ================================================ FILE: fpv_aggressive_trajectories/launch/simulation/simulation.launch ================================================ ================================================ FILE: fpv_aggressive_trajectories/package.xml ================================================ fpv_aggressive_trajectories 0.0.0 The fpv_aggressive_trajectories package Elia Kaufmann TODO Elia Kaufmann catkin catkin_simple acrobatic_trajectory_helper autopilot eigen_catkin minimum_jerk_trajectories quadrotor_common quadrotor_msgs roscpp pose_utils rpg_mpc std_msgs nav_msgs ================================================ FILE: fpv_aggressive_trajectories/parameters/mpc_params.yaml ================================================ # rpg_quadrotor_mpc # A model predictive control implementation for quadrotors. # Copyright (C) 2017-2018 Philipp Foehn, # Robotics and Perception Group, University of Zurich # # Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common. # https://github.com/uzh-rpg/rpg_quadrotor_control # # 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 3 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 . # Cost on states Q_pos_xy: 200 # Cost for horizontal positon error Q_pos_z: 500 # Cost for vertical position error Q_attitude: 50 # Cost for attitude error Q_velocity: 10 # Cost for velocity error Q_perception: 0 # Cost for reprojection error to camera optical axis (PAMPC) # Cost on Inputs R_thrust: 0.1 # Cost on thrust input R_pitchroll: 0.1 # Cost on pitch and roll rate R_yaw: 0.1 # Cost on yaw ratte # Exponential scaling: W_i = W * exp(-i/N * cost_scaling). # cost_scaling = 0 means no scaling W_i = W. state_cost_exponential: 0.0 # Scaling for state costs input_cost_exponential: 0.0 # scaling for input costs # Limits for inputs max_bodyrate_xy: 20.0 # ~ pi [rad/s] max_bodyrate_z: 5.0 # ~ pi*2/3 [rad/s] min_thrust: 1.0 # ~ 20% gravity [N] max_thrust: 40.0 # ~ 200% gravity [N] # Extrinsics for Perception Aware MPC p_B_C: [ 0.0, 0.0, 0.0 ] # camera in body center [m] q_B_C: [ 0.6427876, 0, 0.7660444, 0 ] # 45° pitched down # Print information such as timing to terminal print_info: false ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/asphalt.material ================================================ material iros/asphalt { technique { pass { ambient 0.5 0.5 0.5 1.0 diffuse 0.5 0.5 0.5 1.0 specular 0.2 0.2 0.2 1.0 12.5 texture_unit { texture asphalt.png filtering anistropic max_anisotropy 16 scale 0.1 0.1 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/carpet.material ================================================ material iros/carpet { technique { pass { ambient 0.2 0.2 0.2 1.0 diffuse 0.8 0.8 0.8 1.0 emissive 0.5 0.5 0.5 1 texture_unit { texture carpet.jpg filtering anistropic max_anisotropy 16 scale 1.0 1.0 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_1.material ================================================ material iros/wall_1 { technique { pass { ambient 0.5 0.5 0.5 1.0 diffuse 0.5 0.5 0.5 1.0 emissive 0.5 0.5 0.5 1 texture_unit { texture wall_1.jpeg filtering anistropic max_anisotropy 16 scale 1.0 1.0 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_2.material ================================================ material iros/wall_2 { technique { pass { ambient 0.5 0.5 0.5 1.0 diffuse 0.5 0.5 0.5 1.0 emissive 0.5 0.5 0.5 1 texture_unit { texture wall_2.jpeg filtering anistropic max_anisotropy 16 scale 1.0 1.0 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_3.material ================================================ material iros/wall_3 { technique { pass { ambient 0.5 0.5 0.5 1.0 diffuse 0.5 0.5 0.5 1.0 emissive 0.5 0.5 0.5 1 texture_unit { texture wall_3.jpeg filtering anistropic max_anisotropy 16 scale 1.0 1.0 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_4.material ================================================ material iros/wall_4 { technique { pass { ambient 0.5 0.5 0.5 1.0 diffuse 0.5 0.5 0.5 1.0 emissive 0.5 0.5 0.5 1 texture_unit { texture wall_4.jpeg filtering anistropic max_anisotropy 16 scale 1.0 1.0 } } } } ================================================ FILE: fpv_aggressive_trajectories/resources/race_track/real_world/gate/meshes/gate.dae ================================================ 2018-07-16T17:43:41 2018-07-16T17:43:41 Z_UP -0.25 -0.00513546 1.49999 0.25 -0.00513546 1.49999 0.25 0.00513585 1.49999 -0.25 -0.0154047 1.49989 0.25 -0.0154047 1.49989 0.25 -0.0256704 1.4997 -0.25 -0.0256704 1.4997 -0.25 -0.0359316 1.4994 0.25 -0.0359316 1.4994 -0.25 -0.0461877 1.49901 0.25 -0.0461877 1.49901 0.25 -0.056438 1.49853 -0.25 -0.056438 1.49853 -0.25 -0.0666819 1.49795 0.25 -0.0666819 1.49795 -0.25 -0.0769187 1.49727 0.25 -0.0769187 1.49727 0.25 -0.0871478 1.49649 -0.25 -0.0871478 1.49649 -0.25 -0.0973682 1.49562 0.25 -0.0973682 1.49562 -0.25 -0.10758 1.49465 0.25 -0.10758 1.49465 0.25 -0.117781 1.49358 -0.25 -0.117781 1.49358 -0.25 -0.127972 1.49241 0.25 -0.127972 1.49241 -0.25 -0.138152 1.49116 0.25 -0.138152 1.49116 0.25 -0.148319 1.4898 -0.25 -0.148319 1.4898 -0.25 -0.158474 1.48835 0.25 -0.158474 1.48835 -0.25 -0.168616 1.4868 0.25 -0.168616 1.4868 0.25 -0.178744 1.48515 -0.25 -0.178744 1.48515 -0.25 -0.188857 1.48341 0.25 -0.188857 1.48341 -0.25 -0.198954 1.48157 0.25 -0.198954 1.48157 0.25 -0.209036 1.47964 -0.25 -0.209036 1.47964 -0.25 -0.219101 1.47761 0.25 -0.219101 1.47761 -0.25 -0.229148 1.47549 0.25 -0.229148 1.47549 0.25 -0.239176 1.47327 -0.25 -0.239176 1.47327 -0.25 -0.249182 1.47095 0.25 -0.249182 1.47095 -0.25 -0.259163 1.46854 0.25 -0.259163 1.46854 0.25 -0.269119 1.46604 -0.25 -0.269119 1.46604 -0.25 -0.279048 1.46344 0.25 -0.279048 1.46344 -0.25 -0.288951 1.46075 0.25 -0.288951 1.46075 0.25 -0.298827 1.45797 -0.25 -0.298827 1.45797 -0.25 -0.308675 1.45509 0.25 -0.308675 1.45509 -0.25 -0.318494 1.45212 0.25 -0.318494 1.45212 0.25 -0.328284 1.44906 -0.25 -0.328284 1.44906 -0.25 -0.338045 1.44591 0.25 -0.338045 1.44591 -0.25 -0.347774 1.44266 0.25 -0.347774 1.44266 0.25 -0.357473 1.43932 -0.25 -0.357473 1.43932 -0.25 -0.367139 1.43589 0.25 -0.367139 1.43589 -0.25 -0.376773 1.43237 0.25 -0.376773 1.43237 0.25 -0.386374 1.42876 -0.25 -0.386374 1.42876 -0.25 -0.395942 1.42505 0.25 -0.395942 1.42505 -0.25 -0.405474 1.42126 0.25 -0.405474 1.42126 0.25 -0.414972 1.41737 -0.25 -0.414972 1.41737 -0.25 -0.424433 1.41339 0.25 -0.424433 1.41339 -0.25 -0.433859 1.40933 0.25 -0.433859 1.40933 0.25 -0.443247 1.40517 -0.25 -0.443247 1.40517 -0.25 -0.452598 1.40093 0.25 -0.452598 1.40093 -0.25 -0.461909 1.39659 0.25 -0.461909 1.39659 0.25 -0.471178 1.39217 -0.25 -0.471178 1.39217 -0.25 -0.480402 1.38766 0.25 -0.480402 1.38766 -0.25 -0.489582 1.38306 0.25 -0.489582 1.38306 0.25 -0.498715 1.37838 -0.25 -0.498715 1.37838 -0.25 -0.507803 1.37361 0.25 -0.507803 1.37361 -0.25 -0.516843 1.36876 0.25 -0.516843 1.36876 0.25 -0.525837 1.36382 -0.25 -0.525837 1.36382 -0.25 -0.534782 1.3588 0.25 -0.534782 1.3588 -0.25 -0.543679 1.3537 0.25 -0.543679 1.3537 0.25 -0.552527 1.34851 -0.25 -0.552527 1.34851 -0.25 -0.561324 1.34323 0.25 -0.561324 1.34323 -0.25 -0.570072 1.33788 0.25 -0.570072 1.33788 0.25 -0.578769 1.33244 -0.25 -0.578769 1.33244 -0.25 -0.587414 1.32692 0.25 -0.587414 1.32692 -0.25 -0.596007 1.32131 0.25 -0.596007 1.32131 0.25 -0.604547 1.31563 -0.25 -0.604547 1.31563 -0.25 -0.613034 1.30986 0.25 -0.613034 1.30986 -0.25 -0.621467 1.30402 0.25 -0.621467 1.30402 0.25 -0.629845 1.29809 -0.25 -0.629845 1.29809 -0.25 -0.638169 1.29208 0.25 -0.638169 1.29208 -0.25 -0.646436 1.28599 0.25 -0.646436 1.28599 0.25 -0.654648 1.27983 -0.25 -0.654648 1.27983 -0.25 -0.662802 1.27358 0.25 -0.662802 1.27358 -0.25 -0.670896 1.26726 0.25 -0.670896 1.26726 0.25 -0.678928 1.26086 -0.25 -0.678928 1.26086 -0.25 -0.686897 1.25439 0.25 -0.686897 1.25439 -0.25 -0.694803 1.24785 0.25 -0.694803 1.24785 0.25 -0.702645 1.24123 -0.25 -0.702645 1.24123 -0.25 -0.710423 1.23454 0.25 -0.710423 1.23454 -0.25 -0.718137 1.22777 0.25 -0.718137 1.22777 0.25 -0.725785 1.22093 -0.25 -0.725785 1.22093 -0.25 -0.733368 1.21403 0.25 -0.733368 1.21403 -0.25 -0.740885 1.20705 0.25 -0.740885 1.20705 0.25 -0.748335 1.2 -0.25 -0.748335 1.2 -0.25 -0.755718 1.19288 0.25 -0.755718 1.19288 -0.25 -0.763033 1.18569 0.25 -0.763033 1.18569 0.25 -0.77028 1.17843 -0.25 -0.77028 1.17843 -0.25 -0.777459 1.1711 0.25 -0.777459 1.1711 -0.25 -0.784568 1.1637 0.25 -0.784568 1.1637 0.25 -0.791608 1.15624 -0.25 -0.791608 1.15624 -0.25 -0.798578 1.14871 0.25 -0.798578 1.14871 -0.25 -0.805477 1.14111 0.25 -0.805477 1.14111 0.25 -0.812305 1.13344 -0.25 -0.812305 1.13344 -0.25 -0.819062 1.12571 0.25 -0.819062 1.12571 -0.25 -0.825746 1.11792 0.25 -0.825746 1.11792 0.25 -0.832357 1.11006 -0.25 -0.832357 1.11006 -0.25 -0.838894 1.10214 0.25 -0.838894 1.10214 -0.25 -0.845353 1.09415 0.25 -0.845353 1.09415 0.25 -0.851735 1.08611 -0.25 -0.851735 1.08611 -0.25 -0.858039 1.07801 0.25 -0.858039 1.07801 -0.25 -0.864265 1.06986 0.25 -0.864265 1.06986 0.25 -0.870413 1.06164 -0.25 -0.870413 1.06164 -0.25 -0.876482 1.05337 0.25 -0.876482 1.05337 -0.25 -0.882472 1.04504 0.25 -0.882472 1.04504 0.25 -0.888382 1.03666 -0.25 -0.888382 1.03666 -0.25 -0.894213 1.02822 0.25 -0.894213 1.02822 -0.25 -0.899963 1.01972 0.25 -0.899963 1.01972 0.25 -0.905632 1.01118 -0.25 -0.905632 1.01118 -0.25 -0.91122 1.00257 0.25 -0.91122 1.00257 -0.25 -0.916727 0.993921 0.25 -0.916727 0.993921 0.25 -0.922152 0.985214 -0.25 -0.922152 0.985214 -0.25 -0.927495 0.976456 0.25 -0.927495 0.976456 -0.25 -0.932755 0.967647 0.25 -0.932755 0.967647 0.25 -0.937932 0.958787 -0.25 -0.937932 0.958787 -0.25 -0.943026 0.949877 0.25 -0.943026 0.949877 -0.25 -0.948036 0.940918 0.25 -0.948036 0.940918 0.25 -0.952961 0.93191 -0.25 -0.952961 0.93191 -0.25 -0.957802 0.922854 0.25 -0.957802 0.922854 -0.25 -0.962558 0.913751 0.25 -0.962558 0.913751 0.25 -0.967228 0.904603 -0.25 -0.967228 0.904603 -0.25 -0.971809 0.895413 0.25 -0.971809 0.895413 -0.25 -0.976302 0.886183 0.25 -0.976302 0.886183 0.25 -0.980707 0.876912 -0.25 -0.980707 0.876912 -0.25 -0.985022 0.867601 0.25 -0.985022 0.867601 -0.25 -0.989249 0.858251 0.25 -0.989249 0.858251 0.25 -0.993387 0.848863 -0.25 -0.993387 0.848863 -0.25 -0.997435 0.839437 0.25 -0.997435 0.839437 -0.25 -1.00139 0.829974 0.25 -1.00139 0.829974 0.25 -1.00526 0.820473 -0.25 -1.00526 0.820473 -0.25 -1.00904 0.810938 0.25 -1.00904 0.810938 -0.25 -1.01273 0.801366 0.25 -1.01273 0.801366 0.25 -1.01632 0.791761 -0.25 -1.01632 0.791761 -0.25 -1.01983 0.782121 0.25 -1.01983 0.782121 -0.25 -1.02325 0.772448 0.25 -1.02325 0.772448 0.25 -1.02657 0.762742 -0.25 -1.02657 0.762742 -0.25 -1.0298 0.753004 0.25 -1.0298 0.753004 -0.25 -1.03294 0.743235 0.25 -1.03294 0.743235 0.25 -1.03599 0.733434 -0.25 -1.03599 0.733434 -0.25 -1.03894 0.723604 0.25 -1.03894 0.723604 -0.25 -1.0418 0.713745 0.25 -1.0418 0.713745 0.25 -1.04457 0.703856 -0.25 -1.04457 0.703856 -0.25 -1.04725 0.69394 0.25 -1.04725 0.69394 -0.25 -1.04983 0.683999 0.25 -1.04983 0.683999 0.25 -1.05232 0.674037 -0.25 -1.05232 0.674037 -0.25 -1.05471 0.664053 0.25 -1.05471 0.664053 -0.25 -1.05701 0.654049 0.25 -1.05701 0.654049 0.25 -1.05921 0.644026 -0.25 -1.05921 0.644026 -0.25 -1.06131 0.633983 0.25 -1.06131 0.633983 -0.25 -1.06332 0.623922 0.25 -1.06332 0.623922 0.25 -1.06524 0.613844 -0.25 -1.06524 0.613844 -0.25 -1.06706 0.603749 0.25 -1.06706 0.603749 -0.25 -1.06878 0.593637 0.25 -1.06878 0.593637 0.25 -1.07041 0.58351 -0.25 -1.07041 0.58351 -0.25 -1.07194 0.573368 0.25 -1.07194 0.573368 -0.25 -1.07338 0.563212 0.25 -1.07338 0.563212 0.25 -1.07472 0.553042 -0.25 -1.07472 0.553042 -0.25 -1.07596 0.542859 0.25 -1.07596 0.542859 -0.25 -1.07711 0.532665 0.25 -1.07711 0.532665 0.25 -1.07816 0.522458 -0.25 -1.07816 0.522458 -0.25 -1.07911 0.512242 0.25 -1.07911 0.512242 -0.25 -1.07997 0.502015 0.25 -1.07997 0.502015 0.25 -1.08073 0.491778 -0.25 -1.08073 0.491778 -0.25 -1.08139 0.481533 0.25 -1.08139 0.481533 -0.25 -1.08196 0.47128 0.25 -1.08196 0.47128 0.25 -1.08242 0.46102 -0.25 -1.08242 0.46102 -0.25 -1.0828 0.450756 0.25 -1.0828 0.450756 -0.25 -1.08307 0.440491 0.25 -1.08307 0.440491 0.25 -1.08325 0.430227 -0.25 -1.08325 0.430227 -0.25 -1.08333 0.419963 0.25 -1.08333 0.419963 -0.25 -1.08331 0.4097 0.25 -1.08331 0.4097 0.25 -1.0832 0.39944 -0.25 -1.0832 0.39944 -0.25 -1.08298 0.389182 0.25 -1.08298 0.389182 -0.25 -1.08268 0.378928 0.25 -1.08268 0.378928 0.25 -1.08227 0.368678 -0.25 -1.08227 0.368678 -0.25 -1.08177 0.358433 0.25 -1.08177 0.358433 -0.25 -1.08117 0.348194 0.25 -1.08117 0.348194 0.25 -1.08047 0.33796 -0.25 -1.08047 0.33796 -0.25 -1.07968 0.327734 0.25 -1.07968 0.327734 -0.25 -1.07879 0.317515 0.25 -1.07879 0.317515 0.25 -1.0778 0.307304 -0.25 -1.0778 0.307304 -0.25 -1.07672 0.297103 0.25 -1.07672 0.297103 -0.25 -1.07553 0.286911 0.25 -1.07553 0.286911 0.25 -1.07426 0.27673 -0.25 -1.07426 0.27673 -0.25 -1.07288 0.266559 0.25 -1.07288 0.266559 -0.25 -1.07141 0.256401 0.25 -1.07141 0.256401 0.25 -1.06985 0.246255 -0.25 -1.06985 0.246255 -0.25 -1.06818 0.236122 0.25 -1.06818 0.236122 -0.25 -1.06642 0.226003 0.25 -1.06642 0.226003 0.25 -1.06457 0.215901 -0.25 -1.06457 0.215901 -0.25 -1.06262 0.205819 0.25 -1.06262 0.205819 -0.25 -1.06057 0.195759 0.25 -1.06057 0.195759 0.25 -1.05843 0.185721 -0.25 -1.05843 0.185721 -0.25 -1.0562 0.175704 0.25 -1.0562 0.175704 -0.25 -1.05387 0.165711 0.25 -1.05387 0.165711 0.25 -1.05144 0.155742 -0.25 -1.05144 0.155742 -0.25 -1.04892 0.145797 0.25 -1.04892 0.145797 -0.25 -1.04631 0.135877 0.25 -1.04631 0.135877 0.25 -1.04361 0.125983 -0.25 -1.04361 0.125983 -0.25 -1.04081 0.116115 0.25 -1.04081 0.116115 -0.25 -1.03791 0.106274 0.25 -1.03791 0.106274 0.25 -1.03493 0.0964608 -0.25 -1.03493 0.0964608 -0.25 -1.03185 0.086676 0.25 -1.03185 0.086676 -0.25 -1.02868 0.0769204 0.25 -1.02868 0.0769204 0.25 -1.02542 0.0671944 -0.25 -1.02542 0.0671944 -0.25 -1.02206 0.0574988 0.25 -1.02206 0.0574988 -0.25 -1.01861 0.0478342 0.25 -1.01861 0.0478342 0.25 -1.01507 0.0382013 -0.25 -1.01507 0.0382013 -0.25 -1.01144 0.0286008 0.25 -1.01144 0.0286008 -0.25 -1.00772 0.0190333 0.25 -1.00772 0.0190333 0.25 -1.00391 0.00949949 -0.25 -1.00391 0.00949949 -0.25 -1 5.55112e-17 0.25 -1 5.55112e-17 -0.25 0.00513585 1.49999 0.25 0.015405 1.49989 -0.25 0.015405 1.49989 0.25 0.0256706 1.4997 -0.25 0.0256706 1.4997 -0.25 0.0359317 1.4994 0.25 0.0359317 1.4994 0.25 0.0461877 1.49901 -0.25 0.0461877 1.49901 0.25 0.0564381 1.49853 -0.25 0.0564381 1.49853 -0.25 0.0666821 1.49795 0.25 0.0666821 1.49795 0.25 0.0769189 1.49727 -0.25 0.0769189 1.49727 0.25 0.0871478 1.49649 -0.25 0.0871478 1.49649 -0.25 0.0973683 1.49562 0.25 0.0973683 1.49562 0.25 0.10758 1.49465 -0.25 0.10758 1.49465 0.25 0.117781 1.49358 -0.25 0.117781 1.49358 -0.25 0.127972 1.49241 0.25 0.127972 1.49241 0.25 0.138152 1.49116 -0.25 0.138152 1.49116 0.25 0.148319 1.4898 -0.25 0.148319 1.4898 -0.25 0.158474 1.48835 0.25 0.158474 1.48835 0.25 0.168616 1.4868 -0.25 0.168616 1.4868 0.25 0.178744 1.48515 -0.25 0.178744 1.48515 -0.25 0.188857 1.48341 0.25 0.188857 1.48341 0.25 0.198954 1.48157 -0.25 0.198954 1.48157 0.25 0.209036 1.47964 -0.25 0.209036 1.47964 -0.25 0.219101 1.47761 0.25 0.219101 1.47761 0.25 0.229148 1.47549 -0.25 0.229148 1.47549 0.25 0.239177 1.47327 -0.25 0.239177 1.47327 -0.25 0.249182 1.47095 0.25 0.249182 1.47095 0.25 0.259163 1.46854 -0.25 0.259163 1.46854 0.25 0.269119 1.46604 -0.25 0.269119 1.46604 -0.25 0.279048 1.46344 0.25 0.279048 1.46344 0.25 0.288951 1.46075 -0.25 0.288951 1.46075 0.25 0.298827 1.45797 -0.25 0.298827 1.45797 -0.25 0.308675 1.45509 0.25 0.308675 1.45509 0.25 0.318494 1.45212 -0.25 0.318494 1.45212 0.25 0.328284 1.44906 -0.25 0.328284 1.44906 -0.25 0.338045 1.44591 0.25 0.338045 1.44591 0.25 0.347774 1.44266 -0.25 0.347774 1.44266 0.25 0.357473 1.43932 -0.25 0.357473 1.43932 -0.25 0.367139 1.43589 0.25 0.367139 1.43589 0.25 0.376774 1.43237 -0.25 0.376774 1.43237 0.25 0.386375 1.42876 -0.25 0.386375 1.42876 -0.25 0.395942 1.42505 0.25 0.395942 1.42505 0.25 0.405474 1.42126 -0.25 0.405474 1.42126 0.25 0.414972 1.41737 -0.25 0.414972 1.41737 -0.25 0.424434 1.41339 0.25 0.424434 1.41339 0.25 0.433859 1.40933 -0.25 0.433859 1.40933 0.25 0.443247 1.40517 -0.25 0.443247 1.40517 -0.25 0.452598 1.40093 0.25 0.452598 1.40093 0.25 0.461909 1.39659 -0.25 0.461909 1.39659 0.25 0.471178 1.39217 -0.25 0.471178 1.39217 -0.25 0.480402 1.38766 0.25 0.480402 1.38766 0.25 0.489582 1.38306 -0.25 0.489582 1.38306 0.25 0.498715 1.37838 -0.25 0.498715 1.37838 -0.25 0.507803 1.37361 0.25 0.507803 1.37361 0.25 0.516844 1.36876 -0.25 0.516844 1.36876 0.25 0.525837 1.36382 -0.25 0.525837 1.36382 -0.25 0.534782 1.3588 0.25 0.534782 1.3588 0.25 0.543679 1.3537 -0.25 0.543679 1.3537 0.25 0.552527 1.34851 -0.25 0.552527 1.34851 -0.25 0.561325 1.34323 0.25 0.561325 1.34323 0.25 0.570072 1.33788 -0.25 0.570072 1.33788 0.25 0.578769 1.33244 -0.25 0.578769 1.33244 -0.25 0.587414 1.32692 0.25 0.587414 1.32692 0.25 0.596007 1.32131 -0.25 0.596007 1.32131 0.25 0.604547 1.31563 -0.25 0.604547 1.31563 -0.25 0.613034 1.30986 0.25 0.613034 1.30986 0.25 0.621467 1.30402 -0.25 0.621467 1.30402 0.25 0.629845 1.29809 -0.25 0.629845 1.29809 -0.25 0.638169 1.29208 0.25 0.638169 1.29208 0.25 0.646437 1.28599 -0.25 0.646437 1.28599 0.25 0.654648 1.27983 -0.25 0.654648 1.27983 -0.25 0.662803 1.27358 0.25 0.662803 1.27358 0.25 0.670896 1.26726 -0.25 0.670896 1.26726 0.25 0.678928 1.26086 -0.25 0.678928 1.26086 -0.25 0.686897 1.25439 0.25 0.686897 1.25439 0.25 0.694803 1.24785 -0.25 0.694803 1.24785 0.25 0.702645 1.24123 -0.25 0.702645 1.24123 -0.25 0.710423 1.23454 0.25 0.710423 1.23454 0.25 0.718137 1.22777 -0.25 0.718137 1.22777 0.25 0.725785 1.22093 -0.25 0.725785 1.22093 -0.25 0.733368 1.21403 0.25 0.733368 1.21403 0.25 0.740885 1.20705 -0.25 0.740885 1.20705 0.25 0.748335 1.2 -0.25 0.748335 1.2 -0.25 0.755718 1.19288 0.25 0.755718 1.19288 0.25 0.763033 1.18569 -0.25 0.763033 1.18569 0.25 0.77028 1.17843 -0.25 0.77028 1.17843 -0.25 0.777459 1.1711 0.25 0.777459 1.1711 0.25 0.784568 1.1637 -0.25 0.784568 1.1637 0.25 0.791608 1.15624 -0.25 0.791608 1.15624 -0.25 0.798578 1.14871 0.25 0.798578 1.14871 0.25 0.805477 1.14111 -0.25 0.805477 1.14111 0.25 0.812305 1.13344 -0.25 0.812305 1.13344 -0.25 0.819062 1.12571 0.25 0.819062 1.12571 0.25 0.825746 1.11792 -0.25 0.825746 1.11792 0.25 0.832358 1.11006 -0.25 0.832358 1.11006 -0.25 0.838894 1.10214 0.25 0.838894 1.10214 0.25 0.845353 1.09415 -0.25 0.845353 1.09415 0.25 0.851735 1.08611 -0.25 0.851735 1.08611 -0.25 0.858039 1.07801 0.25 0.858039 1.07801 0.25 0.864265 1.06986 -0.25 0.864265 1.06986 0.25 0.870413 1.06164 -0.25 0.870413 1.06164 -0.25 0.876482 1.05337 0.25 0.876482 1.05337 0.25 0.882472 1.04504 -0.25 0.882472 1.04504 0.25 0.888382 1.03666 -0.25 0.888382 1.03666 -0.25 0.894213 1.02822 0.25 0.894213 1.02822 0.25 0.899963 1.01972 -0.25 0.899963 1.01972 0.25 0.905632 1.01118 -0.25 0.905632 1.01118 -0.25 0.91122 1.00257 0.25 0.91122 1.00257 0.25 0.916727 0.99392 -0.25 0.916727 0.99392 0.25 0.922152 0.985214 -0.25 0.922152 0.985214 -0.25 0.927495 0.976456 0.25 0.927495 0.976456 0.25 0.932755 0.967647 -0.25 0.932755 0.967647 0.25 0.937932 0.958787 -0.25 0.937932 0.958787 -0.25 0.943026 0.949877 0.25 0.943026 0.949877 0.25 0.948036 0.940918 -0.25 0.948036 0.940918 0.25 0.952961 0.93191 -0.25 0.952961 0.93191 -0.25 0.957803 0.922854 0.25 0.957803 0.922854 0.25 0.962559 0.91375 -0.25 0.962559 0.91375 0.25 0.967228 0.904602 -0.25 0.967228 0.904602 -0.25 0.971809 0.895413 0.25 0.971809 0.895413 0.25 0.976302 0.886183 -0.25 0.976302 0.886183 0.25 0.980707 0.876912 -0.25 0.980707 0.876912 -0.25 0.985022 0.867601 0.25 0.985022 0.867601 0.25 0.989249 0.858251 -0.25 0.989249 0.858251 0.25 0.993387 0.848863 -0.25 0.993387 0.848863 -0.25 0.997435 0.839437 0.25 0.997435 0.839437 0.25 1.00139 0.829973 -0.25 1.00139 0.829973 0.25 1.00526 0.820473 -0.25 1.00526 0.820473 -0.25 1.00904 0.810938 0.25 1.00904 0.810938 0.25 1.01273 0.801366 -0.25 1.01273 0.801366 0.25 1.01632 0.79176 -0.25 1.01632 0.79176 -0.25 1.01983 0.782121 0.25 1.01983 0.782121 0.25 1.02325 0.772448 -0.25 1.02325 0.772448 0.25 1.02657 0.762742 -0.25 1.02657 0.762742 -0.25 1.0298 0.753004 0.25 1.0298 0.753004 0.25 1.03294 0.743234 -0.25 1.03294 0.743234 0.25 1.03599 0.733434 -0.25 1.03599 0.733434 -0.25 1.03894 0.723604 0.25 1.03894 0.723604 0.25 1.0418 0.713745 -0.25 1.0418 0.713745 0.25 1.04457 0.703856 -0.25 1.04457 0.703856 -0.25 1.04725 0.693939 0.25 1.04725 0.693939 0.25 1.04983 0.683999 -0.25 1.04983 0.683999 0.25 1.05232 0.674036 -0.25 1.05232 0.674036 -0.25 1.05471 0.664053 0.25 1.05471 0.664053 0.25 1.05701 0.654049 -0.25 1.05701 0.654049 0.25 1.05921 0.644026 -0.25 1.05921 0.644026 -0.25 1.06131 0.633983 0.25 1.06131 0.633983 0.25 1.06332 0.623922 -0.25 1.06332 0.623922 0.25 1.06524 0.613844 -0.25 1.06524 0.613844 -0.25 1.06706 0.603748 0.25 1.06706 0.603748 0.25 1.06878 0.593637 -0.25 1.06878 0.593637 0.25 1.07041 0.58351 -0.25 1.07041 0.58351 -0.25 1.07194 0.573368 0.25 1.07194 0.573368 0.25 1.07338 0.563211 -0.25 1.07338 0.563211 0.25 1.07472 0.553042 -0.25 1.07472 0.553042 -0.25 1.07596 0.542859 0.25 1.07596 0.542859 0.25 1.07711 0.532664 -0.25 1.07711 0.532664 0.25 1.07816 0.522458 -0.25 1.07816 0.522458 -0.25 1.07911 0.512241 0.25 1.07911 0.512241 0.25 1.07997 0.502014 -0.25 1.07997 0.502014 0.25 1.08073 0.491778 -0.25 1.08073 0.491778 -0.25 1.08139 0.481533 0.25 1.08139 0.481533 0.25 1.08196 0.471279 -0.25 1.08196 0.471279 0.25 1.08242 0.461019 -0.25 1.08242 0.461019 -0.25 1.0828 0.450756 0.25 1.0828 0.450756 0.25 1.08307 0.440491 -0.25 1.08307 0.440491 0.25 1.08325 0.430226 -0.25 1.08325 0.430226 -0.25 1.08333 0.419963 0.25 1.08333 0.419963 0.25 1.08331 0.4097 -0.25 1.08331 0.4097 0.25 1.0832 0.39944 -0.25 1.0832 0.39944 -0.25 1.08298 0.389182 0.25 1.08298 0.389182 0.25 1.08268 0.378928 -0.25 1.08268 0.378928 0.25 1.08227 0.368678 -0.25 1.08227 0.368678 -0.25 1.08177 0.358433 0.25 1.08177 0.358433 0.25 1.08117 0.348193 -0.25 1.08117 0.348193 0.25 1.08047 0.33796 -0.25 1.08047 0.33796 -0.25 1.07968 0.327734 0.25 1.07968 0.327734 0.25 1.07879 0.317515 -0.25 1.07879 0.317515 0.25 1.0778 0.307304 -0.25 1.0778 0.307304 -0.25 1.07672 0.297103 0.25 1.07672 0.297103 0.25 1.07553 0.286911 -0.25 1.07553 0.286911 0.25 1.07426 0.27673 -0.25 1.07426 0.27673 -0.25 1.07288 0.266559 0.25 1.07288 0.266559 0.25 1.07141 0.256401 -0.25 1.07141 0.256401 0.25 1.06985 0.246254 -0.25 1.06985 0.246254 -0.25 1.06818 0.236121 0.25 1.06818 0.236121 0.25 1.06642 0.226002 -0.25 1.06642 0.226002 0.25 1.06457 0.215901 -0.25 1.06457 0.215901 -0.25 1.06262 0.205819 0.25 1.06262 0.205819 0.25 1.06057 0.195759 -0.25 1.06057 0.195759 0.25 1.05843 0.18572 -0.25 1.05843 0.18572 -0.25 1.0562 0.175704 0.25 1.0562 0.175704 0.25 1.05387 0.165711 -0.25 1.05387 0.165711 0.25 1.05144 0.155742 -0.25 1.05144 0.155742 -0.25 1.04892 0.145797 0.25 1.04892 0.145797 0.25 1.04631 0.135877 -0.25 1.04631 0.135877 0.25 1.04361 0.125983 -0.25 1.04361 0.125983 -0.25 1.04081 0.116115 0.25 1.04081 0.116115 0.25 1.03791 0.106274 -0.25 1.03791 0.106274 0.25 1.03493 0.0964607 -0.25 1.03493 0.0964607 -0.25 1.03185 0.086676 0.25 1.03185 0.086676 0.25 1.02868 0.0769203 -0.25 1.02868 0.0769203 0.25 1.02542 0.0671943 -0.25 1.02542 0.0671943 -0.25 1.02206 0.0574986 0.25 1.02206 0.0574986 0.25 1.01861 0.0478341 -0.25 1.01861 0.0478341 0.25 1.01507 0.0382012 -0.25 1.01507 0.0382012 -0.25 1.01144 0.0286007 0.25 1.01144 0.0286007 0.25 1.00772 0.0190331 -0.25 1.00772 0.0190331 0.25 1.00391 0.00949949 -0.25 1.00391 0.00949949 0.25 1 3.33067e-16 -0.25 1 3.33067e-16 0.25 0.966092 0.05 -0.25 -0.966092 0.05 0.25 -0.966092 0.05 -0.25 0.966092 0.05 0.25 -0.973093 0.0690057 -0.25 -0.969639 0.0594872 -0.25 -0.973093 0.0690057 -0.25 -0.976452 0.0785552 -0.25 -0.979717 0.0881349 0.25 -0.979717 0.0881349 -0.25 -0.966092 0.05 0.25 -0.969639 0.0594872 0.25 -0.966092 0.05 -0.25 -0.985962 0.107382 -0.25 -0.988942 0.117047 0.25 -0.988942 0.117047 0.25 -0.982887 0.097744 -0.25 -0.982887 0.097744 0.25 -0.985962 0.107382 0.25 -0.997311 0.146206 -0.25 -0.994617 0.13646 -0.25 -0.997311 0.146206 0.25 -0.994617 0.13646 0.25 -0.991827 0.126741 -0.25 -0.991827 0.126741 -0.25 -1.00482 0.17559 0.25 -1.00482 0.17559 -0.25 -1.00241 0.165771 -0.25 -0.999909 0.155976 0.25 -1.00241 0.165771 0.25 -0.999909 0.155976 -0.25 -1.00934 0.195296 -0.25 -1.01146 0.205182 0.25 -1.01146 0.205182 0.25 -1.00713 0.185432 -0.25 -1.00713 0.185432 0.25 -1.00934 0.195296 0.25 -1.01723 0.234963 -0.25 -1.01541 0.225016 -0.25 -1.01723 0.234963 0.25 -1.01541 0.225016 0.25 -1.01348 0.215089 -0.25 -1.01348 0.215089 -0.25 -1.02213 0.264912 0.25 -1.02213 0.264912 -0.25 -1.02059 0.254911 -0.25 -1.01896 0.244928 0.25 -1.02059 0.254911 0.25 -1.01896 0.244928 0.25 -1.02615 0.295011 -0.25 -1.02615 0.295011 0.25 -1.02729 0.305074 0.25 -1.02491 0.284962 -0.25 -1.02357 0.274929 -0.25 -1.02491 0.284962 0.25 -1.03013 0.33532 0.25 -1.02928 0.325232 -0.25 -1.02928 0.325232 -0.25 -1.02833 0.315149 0.25 -1.02833 0.315149 -0.25 -1.02729 0.305074 -0.25 -1.03152 0.355512 0.25 -1.03207 0.365614 0.25 -1.03152 0.355512 -0.25 -1.03013 0.33532 -0.25 -1.03087 0.345414 0.25 -1.03087 0.345414 0.25 -1.03313 0.395937 -0.25 -1.03287 0.385827 -0.25 -1.03313 0.395937 0.25 -1.03252 0.37572 -0.25 -1.03207 0.365614 -0.25 -1.03252 0.37572 -0.25 -1.03329 0.426267 0.25 -1.03329 0.426267 -0.25 -1.03333 0.416157 -0.25 -1.03328 0.406047 0.25 -1.03333 0.416157 0.25 -1.03328 0.406047 -0.25 -1.0329 0.446482 -0.25 -1.03256 0.456586 0.25 -1.03256 0.456586 0.25 -1.03315 0.436376 -0.25 -1.03315 0.436376 0.25 -1.0329 0.446482 0.25 -1.03021 0.496959 0.25 -1.03095 0.486874 -0.25 -1.03095 0.486874 0.25 -1.03158 0.476782 0.25 -1.03212 0.466686 -0.25 -1.03212 0.466686 -0.25 -1.02844 0.517111 0.25 -1.02741 0.527175 0.25 -1.02844 0.517111 -0.25 -1.03021 0.496959 -0.25 -1.02937 0.507038 0.25 -1.02937 0.507038 0.25 -1.02505 0.547277 -0.25 -1.02505 0.547277 0.25 -1.02372 0.557313 0.25 -1.02628 0.53723 -0.25 -1.02741 0.527175 -0.25 -1.02628 0.53723 -0.25 -1.01914 0.587349 0.25 -1.01914 0.587349 -0.25 -1.02076 0.577352 -0.25 -1.02229 0.567339 0.25 -1.02076 0.577352 0.25 -1.02229 0.567339 -0.25 -1.0156 0.607284 -0.25 -1.01368 0.617221 0.25 -1.01368 0.617221 0.25 -1.01742 0.597327 -0.25 -1.01742 0.597327 0.25 -1.0156 0.607284 0.25 -1.00736 0.646901 -0.25 -1.00956 0.637031 -0.25 -1.00736 0.646901 0.25 -1.00956 0.637031 0.25 -1.01167 0.627137 -0.25 -1.01167 0.627137 -0.25 -1.00017 0.676371 0.25 -0.997577 0.686144 0.25 -1.00017 0.676371 -0.25 -1.00506 0.656749 -0.25 -1.00266 0.666572 0.25 -1.00266 0.666572 0.25 -0.992113 0.705611 -0.25 -0.992113 0.705611 0.25 -0.989239 0.715303 0.25 -0.994893 0.695891 -0.25 -0.997577 0.686144 -0.25 -0.994893 0.695891 0.25 -0.980048 0.744207 0.25 -0.983206 0.734602 -0.25 -0.983206 0.734602 -0.25 -0.98627 0.724967 0.25 -0.98627 0.724967 -0.25 -0.989239 0.715303 -0.25 -0.97345 0.763325 -0.25 -0.97001 0.772837 0.25 -0.97001 0.772837 -0.25 -0.980048 0.744207 -0.25 -0.976796 0.753782 0.25 -0.976796 0.753782 0.25 -0.95913 0.801175 -0.25 -0.96285 0.791763 -0.25 -0.95913 0.801175 0.25 -0.96285 0.791763 0.25 -0.966477 0.782317 -0.25 -0.966477 0.782317 -0.25 -0.947414 0.829201 0.25 -0.947414 0.829201 -0.25 -0.951412 0.819895 -0.25 -0.955318 0.810553 0.25 -0.951412 0.819895 0.25 -0.955318 0.810553 -0.25 -0.939146 0.847691 -0.25 -0.934879 0.856871 0.25 -0.934879 0.856871 0.25 -0.943325 0.838468 -0.25 -0.943325 0.838468 0.25 -0.939146 0.847691 0.25 -0.916926 0.893138 0.25 -0.921545 0.88414 -0.25 -0.921545 0.88414 0.25 -0.926078 0.875096 0.25 -0.930522 0.866006 -0.25 -0.930522 0.866006 -0.25 -0.907425 0.910993 0.25 -0.902544 0.919848 0.25 -0.907425 0.910993 -0.25 -0.916926 0.893138 -0.25 -0.912218 0.902089 0.25 -0.912218 0.902089 0.25 -0.892526 0.937412 -0.25 -0.892526 0.937412 0.25 -0.887389 0.946118 0.25 -0.897578 0.928655 -0.25 -0.902544 0.919848 -0.25 -0.897578 0.928655 0.25 -0.871469 0.971932 0.25 -0.876859 0.96338 -0.25 -0.876859 0.96338 -0.25 -0.882166 0.954775 0.25 -0.882166 0.954775 -0.25 -0.887389 0.946118 -0.25 -0.860436 0.98888 0.25 -0.854795 0.997273 0.25 -0.860436 0.98888 -0.25 -0.871469 0.971932 -0.25 -0.865994 0.980433 0.25 -0.865994 0.980433 0.25 -0.843265 1.0139 -0.25 -0.843265 1.0139 0.25 -0.837377 1.02212 0.25 -0.849071 1.00561 -0.25 -0.854795 0.997273 -0.25 -0.849071 1.00561 0.25 -0.819225 1.04647 0.25 -0.825356 1.03841 -0.25 -0.825356 1.03841 -0.25 -0.831407 1.0303 0.25 -0.831407 1.0303 -0.25 -0.837377 1.02212 -0.25 -0.806722 1.0624 0.25 -0.800355 1.07028 0.25 -0.806722 1.0624 -0.25 -0.819225 1.04647 -0.25 -0.813013 1.05447 0.25 -0.813013 1.05447 0.25 -0.787396 1.08583 -0.25 -0.787396 1.08583 0.25 -0.780806 1.09351 0.25 -0.793913 1.07809 -0.25 -0.800355 1.07028 -0.25 -0.793913 1.07809 0.25 -0.767404 1.10867 -0.25 -0.774141 1.10112 -0.25 -0.767404 1.10867 0.25 -0.774141 1.10112 -0.25 -0.780806 1.09351 -0.25 -0.74676 1.1309 0.25 -0.74676 1.1309 -0.25 -0.753713 1.12356 -0.25 -0.760594 1.11615 0.25 -0.753713 1.12356 0.25 -0.760594 1.11615 -0.25 -0.732641 1.14537 -0.25 -0.725477 1.15251 0.25 -0.725477 1.15251 0.25 -0.739736 1.13817 -0.25 -0.739736 1.13817 0.25 -0.732641 1.14537 0.25 -0.70357 1.17348 -0.25 -0.710941 1.16656 -0.25 -0.70357 1.17348 0.25 -0.710941 1.16656 0.25 -0.718244 1.15957 -0.25 -0.718244 1.15957 0.25 -0.681054 1.1938 0.25 -0.688626 1.1871 -0.25 -0.688626 1.1871 -0.25 -0.696132 1.18033 0.25 -0.696132 1.18033 -0.25 -0.66571 1.20699 0.25 -0.657941 1.21347 0.25 -0.66571 1.20699 -0.25 -0.681054 1.1938 -0.25 -0.673415 1.20043 0.25 -0.673415 1.20043 -0.25 -0.650106 1.21987 -0.25 -0.642208 1.2262 0.25 -0.642208 1.2262 0.25 -0.650106 1.21987 -0.25 -0.657941 1.21347 0.25 -0.618133 1.24473 -0.25 -0.626221 1.23863 -0.25 -0.618133 1.24473 0.25 -0.626221 1.23863 0.25 -0.634246 1.23245 -0.25 -0.634246 1.23245 0.25 -0.593529 1.26254 0.25 -0.601786 1.25669 -0.25 -0.601786 1.25669 -0.25 -0.609988 1.25075 0.25 -0.609988 1.25075 -0.25 -0.57685 1.274 0.25 -0.568429 1.27961 0.25 -0.57685 1.274 -0.25 -0.593529 1.26254 -0.25 -0.585216 1.26831 0.25 -0.585216 1.26831 -0.25 -0.559956 1.28513 -0.25 -0.55143 1.29057 0.25 -0.55143 1.29057 -0.25 -0.568429 1.27961 0.25 -0.559956 1.28513 0.25 -0.534223 1.30119 -0.25 -0.534223 1.30119 0.25 -0.525543 1.30638 0.25 -0.542852 1.29592 -0.25 -0.542852 1.29592 0.25 -0.508035 1.31649 -0.25 -0.516814 1.31147 -0.25 -0.508035 1.31649 0.25 -0.516814 1.31147 -0.25 -0.525543 1.30638 -0.25 -0.481409 1.33101 0.25 -0.481409 1.33101 -0.25 -0.490332 1.32626 -0.25 -0.499207 1.32142 0.25 -0.490332 1.32626 0.25 -0.499207 1.32142 -0.25 -0.463423 1.34026 0.25 -0.454362 1.34475 0.25 -0.463423 1.34026 -0.25 -0.472439 1.33568 0.25 -0.472439 1.33568 -0.25 -0.445256 1.34915 -0.25 -0.436106 1.35346 0.25 -0.436106 1.35346 -0.25 -0.454362 1.34475 0.25 -0.445256 1.34915 0.25 -0.417675 1.36183 -0.25 -0.417675 1.36183 0.25 -0.408395 1.36587 0.25 -0.426912 1.35769 -0.25 -0.426912 1.35769 0.25 -0.380313 1.37747 0.25 -0.389713 1.37369 -0.25 -0.389713 1.37369 -0.25 -0.399075 1.36983 0.25 -0.399075 1.36983 -0.25 -0.408395 1.36587 -0.25 -0.361414 1.38474 0.25 -0.361414 1.38474 -0.25 -0.37088 1.38115 -0.25 -0.380313 1.37747 0.25 -0.37088 1.38115 -0.25 -0.342385 1.39163 0.25 -0.332823 1.39493 0.25 -0.342385 1.39163 0.25 -0.351915 1.38823 -0.25 -0.351915 1.38823 0.25 -0.313611 1.40126 -0.25 -0.313611 1.40126 0.25 -0.303961 1.40428 0.25 -0.323232 1.39814 -0.25 -0.332823 1.39493 -0.25 -0.323232 1.39814 0.25 -0.284577 1.41004 -0.25 -0.294283 1.40721 -0.25 -0.284577 1.41004 0.25 -0.294283 1.40721 -0.25 -0.303961 1.40428 -0.25 -0.255303 1.41796 0.25 -0.255303 1.41796 -0.25 -0.265086 1.41542 -0.25 -0.274845 1.41278 0.25 -0.265086 1.41542 0.25 -0.274845 1.41278 -0.25 -0.235662 1.42277 0.25 -0.225807 1.42503 0.25 -0.235662 1.42277 0.25 -0.245494 1.42041 -0.25 -0.245494 1.42041 0.25 -0.206029 1.42925 -0.25 -0.206029 1.42925 0.25 -0.196108 1.43122 0.25 -0.215929 1.42719 -0.25 -0.225807 1.42503 -0.25 -0.215929 1.42719 0.25 -0.166226 1.43654 0.25 -0.176206 1.43487 -0.25 -0.176206 1.43487 -0.25 -0.186166 1.43309 0.25 -0.186166 1.43309 -0.25 -0.196108 1.43122 -0.25 -0.146211 1.4396 0.25 -0.136179 1.44099 0.25 -0.146211 1.4396 -0.25 -0.166226 1.43654 -0.25 -0.156227 1.43812 0.25 -0.156227 1.43812 -0.25 -0.126132 1.44227 -0.25 -0.116074 1.44346 0.25 -0.116074 1.44346 -0.25 -0.136179 1.44099 0.25 -0.126132 1.44227 0.25 -0.085856 1.44643 -0.25 -0.0959358 1.44554 -0.25 -0.085856 1.44643 0.25 -0.0959358 1.44554 0.25 -0.106009 1.44455 -0.25 -0.106009 1.44455 -0.25 -0.0555819 1.4485 0.25 -0.0555819 1.4485 -0.25 -0.0656783 1.44791 -0.25 -0.0757699 1.44722 0.25 -0.0656783 1.44791 0.25 -0.0757699 1.44722 -0.25 -0.035378 1.44939 -0.25 -0.0252719 1.44969 0.25 -0.0252719 1.44969 0.25 -0.0454816 1.449 -0.25 -0.0454816 1.449 0.25 -0.035378 1.44939 0.25 0.00505468 1.44999 -0.25 -0.00505475 1.44999 -0.25 0.00505468 1.44999 0.25 -0.00505475 1.44999 0.25 -0.0151639 1.44989 -0.25 -0.0151639 1.44989 -0.25 0.0353779 1.44939 0.25 0.0353779 1.44939 -0.25 0.0252717 1.44969 -0.25 0.0151637 1.44989 0.25 0.0252717 1.44969 0.25 0.0151637 1.44989 -0.25 0.0555818 1.4485 -0.25 0.0656781 1.44791 0.25 0.0656781 1.44791 0.25 0.0454815 1.449 -0.25 0.0454815 1.449 0.25 0.0555818 1.4485 0.25 0.0959358 1.44554 -0.25 0.0858559 1.44643 -0.25 0.0959358 1.44554 0.25 0.0858559 1.44643 0.25 0.0757697 1.44722 -0.25 0.0757697 1.44722 -0.25 0.126131 1.44227 0.25 0.126131 1.44227 -0.25 0.116074 1.44346 -0.25 0.106009 1.44455 0.25 0.116074 1.44346 0.25 0.106009 1.44455 0.25 0.156227 1.43812 -0.25 0.156227 1.43812 0.25 0.166225 1.43654 0.25 0.136178 1.44099 -0.25 0.136178 1.44099 0.25 0.146211 1.4396 0.25 0.196108 1.43122 0.25 0.186166 1.43309 -0.25 0.186166 1.43309 -0.25 0.176205 1.43487 0.25 0.176205 1.43487 -0.25 0.166225 1.43654 -0.25 0.215928 1.42719 0.25 0.225806 1.42503 0.25 0.215928 1.42719 -0.25 0.196108 1.43122 -0.25 0.206029 1.42925 0.25 0.206029 1.42925 0.25 0.245494 1.42041 -0.25 0.245494 1.42041 0.25 0.255302 1.41796 0.25 0.235662 1.42277 -0.25 0.225806 1.42503 -0.25 0.235662 1.42277 -0.25 0.284577 1.41004 0.25 0.284577 1.41004 -0.25 0.274845 1.41278 -0.25 0.265086 1.41542 0.25 0.265086 1.41542 -0.25 0.255302 1.41796 -0.25 0.303961 1.40428 -0.25 0.313611 1.40126 0.25 0.313611 1.40126 0.25 0.294282 1.40721 -0.25 0.294282 1.40721 0.25 0.303961 1.40428 0.25 0.342385 1.39163 -0.25 0.332823 1.39493 -0.25 0.342385 1.39163 0.25 0.332823 1.39493 0.25 0.323232 1.39814 -0.25 0.323232 1.39814 -0.25 0.37088 1.38115 0.25 0.37088 1.38115 -0.25 0.361414 1.38474 -0.25 0.351915 1.38823 0.25 0.361414 1.38474 0.25 0.351915 1.38823 0.25 0.399074 1.36983 -0.25 0.399074 1.36983 0.25 0.408395 1.36587 0.25 0.389712 1.37369 -0.25 0.380313 1.37747 -0.25 0.389712 1.37369 0.25 0.436105 1.35346 0.25 0.426911 1.35769 -0.25 0.426911 1.35769 -0.25 0.417675 1.36183 0.25 0.417675 1.36183 -0.25 0.408395 1.36587 -0.25 0.454362 1.34475 -0.25 0.463423 1.34026 0.25 0.463423 1.34026 -0.25 0.436105 1.35346 -0.25 0.445256 1.34915 0.25 0.445256 1.34915 0.25 0.490332 1.32626 -0.25 0.481409 1.33101 -0.25 0.490332 1.32626 0.25 0.481409 1.33101 0.25 0.472439 1.33568 -0.25 0.472439 1.33568 -0.25 0.516813 1.31147 0.25 0.516813 1.31147 -0.25 0.508035 1.31649 -0.25 0.499207 1.32142 0.25 0.508035 1.31649 0.25 0.499207 1.32142 0.25 0.542852 1.29592 -0.25 0.542852 1.29592 0.25 0.55143 1.29057 0.25 0.525543 1.30638 -0.25 0.525543 1.30638 0.25 0.534223 1.30119 0.25 0.57685 1.274 0.25 0.568429 1.27961 -0.25 0.568429 1.27961 -0.25 0.559956 1.28513 0.25 0.559956 1.28513 -0.25 0.55143 1.29057 -0.25 0.593529 1.26254 0.25 0.601786 1.25669 0.25 0.593529 1.26254 -0.25 0.57685 1.274 -0.25 0.585216 1.26831 0.25 0.585216 1.26831 0.25 0.62622 1.23863 -0.25 0.618133 1.24473 -0.25 0.62622 1.23863 0.25 0.609988 1.25075 -0.25 0.601786 1.25669 -0.25 0.609988 1.25075 -0.25 0.650106 1.21987 0.25 0.650106 1.21987 -0.25 0.642208 1.2262 -0.25 0.634246 1.23245 0.25 0.642208 1.2262 0.25 0.634246 1.23245 -0.25 0.66571 1.20699 -0.25 0.673415 1.20043 0.25 0.673415 1.20043 0.25 0.657941 1.21347 -0.25 0.657941 1.21347 0.25 0.66571 1.20699 0.25 0.696132 1.18033 -0.25 0.688626 1.1871 -0.25 0.696132 1.18033 0.25 0.688626 1.1871 0.25 0.681053 1.1938 -0.25 0.681053 1.1938 -0.25 0.718243 1.15957 0.25 0.725477 1.15251 0.25 0.718243 1.15957 -0.25 0.70357 1.17348 -0.25 0.710941 1.16656 0.25 0.710941 1.16656 0.25 0.739736 1.13817 -0.25 0.739736 1.13817 0.25 0.74676 1.1309 0.25 0.732641 1.14537 -0.25 0.725477 1.15251 -0.25 0.732641 1.14537 0.25 0.767404 1.10867 0.25 0.760594 1.11615 -0.25 0.760594 1.11615 -0.25 0.753713 1.12356 0.25 0.753713 1.12356 -0.25 0.74676 1.1309 -0.25 0.780805 1.09351 0.25 0.787396 1.08583 0.25 0.780805 1.09351 -0.25 0.767404 1.10867 -0.25 0.774141 1.10112 0.25 0.774141 1.10112 0.25 0.800355 1.07028 -0.25 0.800355 1.07028 0.25 0.806722 1.0624 0.25 0.793913 1.07809 -0.25 0.787396 1.08583 -0.25 0.793913 1.07809 0.25 0.825356 1.03841 0.25 0.819225 1.04647 -0.25 0.819225 1.04647 -0.25 0.813012 1.05447 0.25 0.813012 1.05447 -0.25 0.806722 1.0624 -0.25 0.837377 1.02212 0.25 0.843265 1.0139 0.25 0.837377 1.02212 -0.25 0.825356 1.03841 -0.25 0.831407 1.0303 0.25 0.831407 1.0303 -0.25 0.849071 1.00561 -0.25 0.854795 0.997273 0.25 0.854795 0.997273 0.25 0.849071 1.00561 -0.25 0.843265 1.0139 -0.25 0.860436 0.98888 0.25 0.860436 0.98888 0.25 0.865994 0.980433 -0.25 0.865994 0.980433 -0.25 0.871469 0.971932 0.25 0.871469 0.971932 -0.25 0.876859 0.96338 0.25 0.876859 0.96338 0.25 0.882166 0.954775 -0.25 0.882166 0.954775 -0.25 0.887388 0.946119 0.25 0.887388 0.946119 -0.25 0.892526 0.937412 0.25 0.892526 0.937412 0.25 0.897578 0.928655 -0.25 0.897578 0.928655 -0.25 0.902544 0.919848 0.25 0.902544 0.919848 -0.25 0.907424 0.910993 0.25 0.907424 0.910993 0.25 0.912218 0.90209 -0.25 0.912218 0.90209 -0.25 0.916925 0.893138 0.25 0.916925 0.893138 -0.25 0.921545 0.88414 0.25 0.921545 0.88414 0.25 0.926078 0.875096 -0.25 0.926078 0.875096 -0.25 0.930522 0.866006 0.25 0.930522 0.866006 -0.25 0.934879 0.856871 0.25 0.934879 0.856871 0.25 0.939146 0.847691 -0.25 0.939146 0.847691 -0.25 0.943325 0.838468 0.25 0.943325 0.838468 -0.25 0.947414 0.829201 0.25 0.947414 0.829201 0.25 0.951412 0.819895 -0.25 0.951412 0.819895 -0.25 0.955317 0.810553 0.25 0.955317 0.810553 -0.25 0.95913 0.801175 0.25 0.95913 0.801175 0.25 0.96285 0.791763 -0.25 0.96285 0.791763 -0.25 0.966477 0.782317 0.25 0.966477 0.782317 -0.25 0.97001 0.772838 0.25 0.97001 0.772838 0.25 0.97345 0.763326 -0.25 0.97345 0.763326 -0.25 0.976796 0.753782 0.25 0.976796 0.753782 -0.25 0.980048 0.744207 0.25 0.980048 0.744207 0.25 0.983206 0.734602 -0.25 0.983206 0.734602 -0.25 0.98627 0.724967 0.25 0.98627 0.724967 -0.25 0.989239 0.715303 0.25 0.989239 0.715303 0.25 0.992113 0.705611 -0.25 0.992113 0.705611 -0.25 0.994893 0.695891 0.25 0.994893 0.695891 -0.25 0.997577 0.686144 0.25 0.997577 0.686144 0.25 1.00017 0.676371 -0.25 1.00017 0.676371 -0.25 1.00266 0.666572 0.25 1.00266 0.666572 -0.25 1.00506 0.656749 0.25 1.00506 0.656749 0.25 1.00736 0.646902 -0.25 1.00736 0.646902 -0.25 1.00956 0.637031 0.25 1.00956 0.637031 -0.25 1.01167 0.627137 0.25 1.01167 0.627137 0.25 1.01368 0.617221 -0.25 1.01368 0.617221 -0.25 1.0156 0.607284 0.25 1.0156 0.607284 -0.25 1.01742 0.597327 0.25 1.01742 0.597327 0.25 1.01914 0.587349 -0.25 1.01914 0.587349 -0.25 1.02076 0.577353 0.25 1.02076 0.577353 -0.25 1.02229 0.567339 0.25 1.02229 0.567339 0.25 1.02372 0.557313 -0.25 1.02372 0.557313 -0.25 1.02505 0.547277 0.25 1.02505 0.547277 -0.25 1.02628 0.537231 0.25 1.02628 0.537231 0.25 1.02741 0.527175 -0.25 1.02741 0.527175 -0.25 1.02844 0.517111 0.25 1.02844 0.517111 -0.25 1.02937 0.507039 0.25 1.02937 0.507039 0.25 1.03021 0.496959 -0.25 1.03021 0.496959 -0.25 1.03095 0.486874 0.25 1.03095 0.486874 -0.25 1.03158 0.476783 0.25 1.03158 0.476783 0.25 1.03212 0.466686 -0.25 1.03212 0.466686 -0.25 1.03256 0.456586 0.25 1.03256 0.456586 -0.25 1.0329 0.446482 0.25 1.0329 0.446482 0.25 1.03315 0.436376 -0.25 1.03315 0.436376 -0.25 1.03329 0.426267 0.25 1.03329 0.426267 -0.25 1.03333 0.416157 0.25 1.03333 0.416157 0.25 1.03328 0.406047 -0.25 1.03328 0.406047 -0.25 1.03313 0.395937 0.25 1.03313 0.395937 -0.25 1.03287 0.385827 0.25 1.03287 0.385827 0.25 1.03252 0.37572 -0.25 1.03252 0.37572 -0.25 1.03207 0.365615 0.25 1.03207 0.365615 -0.25 1.03152 0.355512 0.25 1.03152 0.355512 0.25 1.03087 0.345414 -0.25 1.03087 0.345414 -0.25 1.03013 0.33532 0.25 1.03013 0.33532 -0.25 1.02928 0.325232 0.25 1.02928 0.325232 0.25 1.02833 0.315149 -0.25 1.02833 0.315149 -0.25 1.02729 0.305075 0.25 1.02729 0.305075 -0.25 1.02615 0.295011 0.25 1.02615 0.295011 0.25 1.02491 0.284962 -0.25 1.02491 0.284962 -0.25 1.02357 0.274929 0.25 1.02357 0.274929 -0.25 1.02213 0.264912 0.25 1.02213 0.264912 0.25 1.02059 0.254911 -0.25 1.02059 0.254911 -0.25 1.01896 0.244928 0.25 1.01896 0.244928 -0.25 1.01723 0.234963 0.25 1.01723 0.234963 0.25 1.01541 0.225016 -0.25 1.01541 0.225016 -0.25 1.01348 0.215089 0.25 1.01348 0.215089 -0.25 1.01146 0.205182 0.25 1.01146 0.205182 0.25 1.00934 0.195297 -0.25 1.00934 0.195297 -0.25 1.00713 0.185432 0.25 1.00713 0.185432 -0.25 1.00482 0.17559 0.25 1.00482 0.17559 0.25 1.00241 0.165772 -0.25 1.00241 0.165772 -0.25 0.999909 0.155976 0.25 0.999909 0.155976 -0.25 0.997311 0.146206 0.25 0.997311 0.146206 0.25 0.994617 0.13646 -0.25 0.994617 0.13646 -0.25 0.991827 0.126741 0.25 0.991827 0.126741 -0.25 0.988942 0.117048 0.25 0.988942 0.117048 0.25 0.985962 0.107382 -0.25 0.985962 0.107382 -0.25 0.982887 0.097744 0.25 0.982887 0.097744 -0.25 0.979717 0.088135 0.25 0.979717 0.088135 0.25 0.976452 0.0785554 -0.25 0.976452 0.0785554 -0.25 0.973093 0.0690059 0.25 0.973093 0.0690059 -0.25 0.969639 0.0594872 0.25 0.969639 0.0594872 0.25 0.966092 0.05 -0.25 0.966092 0.05 0.25 0.70357 1.17348 0.25 0.618133 1.24473 -0.25 0.534223 1.30119 0.25 0.454362 1.34475 0.25 0.380313 1.37747 0.25 0.274845 1.41278 -0.25 0.146211 1.4396 -0.25 -0.926078 0.875096 0.25 -0.97345 0.763325 0.25 -1.00506 0.656749 -0.25 -1.02372 0.557313 -0.25 -1.03158 0.476782 0.25 -1.03287 0.385827 0.25 -1.02357 0.274929 0.25 -0.976452 0.0785552 -0.25 1 3.33067e-16 0.25 1 3.33067e-16 0.25 -1 5.55112e-17 -0.25 -1 5.55112e-17 0.25 0.596007 1.32131 0.25 0.55143 1.29057 0.25 0.559956 1.28513 0.25 -0.800355 1.07028 0.25 -0.845353 1.09415 0.25 -0.851735 1.08611 0.25 -0.812305 1.13344 0.25 -0.819062 1.12571 0.25 -0.767404 1.10867 0.25 -0.825746 1.11792 0.25 -0.774141 1.10112 0.25 -0.74676 1.1309 0.25 -0.791608 1.15624 0.25 -0.798578 1.14871 0.25 0.516813 1.31147 0.25 0.525543 1.30638 0.25 0.561325 1.34323 0.25 -0.777459 1.1711 0.25 -0.725477 1.15251 0.25 -0.77028 1.17843 0.25 0.543679 1.3537 0.25 0.534782 1.3588 0.25 0.499207 1.32142 0.25 -0.70357 1.17348 0.25 -0.748335 1.2 0.25 -0.755718 1.19288 0.25 0.472439 1.33568 0.25 0.516844 1.36876 0.25 0.507803 1.37361 0.25 -0.733368 1.21403 0.25 -0.681054 1.1938 0.25 -0.725785 1.22093 0.25 0.480402 1.38766 0.25 0.445256 1.34915 0.25 0.489582 1.38306 0.25 0.443247 1.40517 0.25 0.433859 1.40933 0.25 0.399074 1.36983 0.25 0.461909 1.39659 0.25 0.452598 1.40093 0.25 0.417675 1.36183 0.25 -0.634246 1.23245 0.25 -0.678928 1.26086 0.25 -0.686897 1.25439 0.25 0.424434 1.41339 0.25 0.389712 1.37369 0.25 -0.662802 1.27358 0.25 -0.609988 1.25075 0.25 -0.654648 1.27983 0.25 0.361414 1.38474 0.25 0.37088 1.38115 0.25 0.395942 1.42505 0.25 0.347774 1.44266 0.25 0.313611 1.40126 0.25 0.323232 1.39814 0.25 0.367139 1.43589 0.25 0.332823 1.39493 0.25 0.342385 1.39163 0.25 -0.568429 1.27961 0.25 -0.604547 1.31563 0.25 -0.613034 1.30986 0.25 0.338045 1.44591 0.25 0.303961 1.40428 0.25 -0.587414 1.32692 0.25 -0.542852 1.29592 0.25 -0.578769 1.33244 0.25 0.284577 1.41004 0.25 0.308675 1.45509 0.25 0.274845 1.41278 0.25 -0.561324 1.34323 0.25 -0.516814 1.31147 0.25 -0.552527 1.34851 0.25 0.245494 1.42041 0.25 0.255302 1.41796 0.25 0.279048 1.46344 0.25 -0.525837 1.36382 0.25 -0.534782 1.3588 0.25 -0.490332 1.32626 0.25 0.235662 1.42277 0.25 0.249182 1.47095 0.25 0.225806 1.42503 0.25 -0.498715 1.37838 0.25 -0.507803 1.37361 0.25 -0.463423 1.34026 0.25 0.196108 1.43122 0.25 0.206029 1.42925 0.25 0.219101 1.47761 0.25 -0.436106 1.35346 0.25 -0.471178 1.39217 0.25 -0.480402 1.38766 0.25 -0.489582 1.38306 0.25 -0.445256 1.34915 0.25 -0.408395 1.36587 0.25 -0.443247 1.40517 0.25 -0.452598 1.40093 0.25 0.136178 1.44099 0.25 0.146211 1.4396 0.25 0.158474 1.48835 0.25 -0.389713 1.37369 0.25 -0.380313 1.37747 0.25 -0.414972 1.41737 0.25 0.117781 1.49358 0.25 0.116074 1.44346 0.25 0.127972 1.49241 0.25 -0.361414 1.38474 0.25 -0.351915 1.38823 0.25 -0.386374 1.42876 0.25 0.0871478 1.49649 0.25 0.0858559 1.44643 0.25 0.0973683 1.49562 0.25 -0.367139 1.43589 0.25 -0.332823 1.39493 0.25 -0.357473 1.43932 0.25 -0.342385 1.39163 0.25 -0.338045 1.44591 0.25 -0.303961 1.40428 0.25 -0.328284 1.44906 0.25 0.0256706 1.4997 0.25 0.0252717 1.44969 0.25 0.0359317 1.4994 0.25 0.00505468 1.44999 0.25 -0.00513546 1.49999 0.25 -0.00505475 1.44999 0.25 -0.0359316 1.4994 0.25 -0.035378 1.44939 0.25 -0.0252719 1.44969 0.25 -0.308675 1.45509 0.25 -0.274845 1.41278 0.25 -0.298827 1.45797 0.25 -0.0555819 1.4485 0.25 -0.056438 1.49853 0.25 -0.0666819 1.49795 0.25 -0.245494 1.42041 0.25 -0.269119 1.46604 0.25 -0.279048 1.46344 0.25 -0.0871478 1.49649 0.25 -0.0973682 1.49562 0.25 -0.085856 1.44643 0.25 -0.215929 1.42719 0.25 -0.239176 1.47327 0.25 -0.225807 1.42503 0.25 -0.176206 1.43487 0.25 -0.166226 1.43654 0.25 -0.188857 1.48341 0.25 -0.10758 1.49465 0.25 -0.0959358 1.44554 0.25 -0.158474 1.48835 0.25 -0.146211 1.4396 0.25 -0.136179 1.44099 0.25 -0.138152 1.49116 0.25 -0.126132 1.44227 0.25 -0.116074 1.44346 0.25 -0.168616 1.4868 0.25 -0.156227 1.43812 0.25 -0.148319 1.4898 0.25 -0.106009 1.44455 0.25 -0.127972 1.49241 0.25 -0.178744 1.48515 0.25 -0.117781 1.49358 0.25 -0.198954 1.48157 0.25 -0.186166 1.43309 0.25 -0.206029 1.42925 0.25 -0.196108 1.43122 0.25 -0.219101 1.47761 0.25 -0.209036 1.47964 0.25 -0.0757699 1.44722 0.25 -0.0769187 1.49727 0.25 -0.229148 1.47549 0.25 -0.235662 1.42277 0.25 -0.249182 1.47095 0.25 -0.0656783 1.44791 0.25 -0.0454816 1.449 0.25 -0.0461877 1.49901 0.25 -0.259163 1.46854 0.25 -0.288951 1.46075 0.25 -0.255303 1.41796 0.25 -0.0151639 1.44989 0.25 -0.0256704 1.4997 0.25 -0.265086 1.41542 0.25 -0.318494 1.45212 0.25 -0.284577 1.41004 0.25 -0.0154047 1.49989 0.25 0.015405 1.49989 0.25 0.00513585 1.49999 0.25 -0.294283 1.40721 0.25 -0.347774 1.44266 0.25 -0.313611 1.40126 0.25 0.0151637 1.44989 0.25 0.0353779 1.44939 0.25 0.0461877 1.49901 0.25 -0.323232 1.39814 0.25 0.0555818 1.4485 0.25 0.0666821 1.49795 0.25 0.0564381 1.49853 0.25 0.0454815 1.449 0.25 0.0656781 1.44791 0.25 0.0769189 1.49727 0.25 -0.376773 1.43237 0.25 -0.395942 1.42505 0.25 -0.37088 1.38115 0.25 0.0757697 1.44722 0.25 0.0959358 1.44554 0.25 0.10758 1.49465 0.25 -0.405474 1.42126 0.25 -0.433859 1.40933 0.25 -0.424433 1.41339 0.25 0.106009 1.44455 0.25 0.126131 1.44227 0.25 0.138152 1.49116 0.25 -0.399075 1.36983 0.25 -0.461909 1.39659 0.25 -0.417675 1.36183 0.25 0.148319 1.4898 0.25 0.156227 1.43812 0.25 0.168616 1.4868 0.25 -0.426912 1.35769 0.25 0.176205 1.43487 0.25 0.188857 1.48341 0.25 0.166225 1.43654 0.25 0.178744 1.48515 0.25 0.186166 1.43309 0.25 0.198954 1.48157 0.25 -0.454362 1.34475 0.25 -0.472439 1.33568 0.25 -0.516843 1.36876 0.25 0.209036 1.47964 0.25 0.215928 1.42719 0.25 0.229148 1.47549 0.25 -0.481409 1.33101 0.25 -0.543679 1.3537 0.25 -0.499207 1.32142 0.25 0.239177 1.47327 0.25 0.259163 1.46854 0.25 -0.508035 1.31649 0.25 -0.570072 1.33788 0.25 -0.525543 1.30638 0.25 0.269119 1.46604 0.25 0.265086 1.41542 0.25 0.288951 1.46075 0.25 -0.534223 1.30119 0.25 -0.596007 1.32131 0.25 -0.55143 1.29057 0.25 0.298827 1.45797 0.25 0.294282 1.40721 0.25 0.318494 1.45212 0.25 -0.559956 1.28513 0.25 -0.621467 1.30402 0.25 -0.57685 1.274 0.25 0.328284 1.44906 0.25 -0.593529 1.26254 0.25 -0.629845 1.29809 0.25 -0.638169 1.29208 0.25 -0.585216 1.26831 0.25 -0.601786 1.25669 0.25 -0.646436 1.28599 0.25 0.357473 1.43932 0.25 0.351915 1.38823 0.25 0.376774 1.43237 0.25 -0.670896 1.26726 0.25 -0.618133 1.24473 0.25 0.386375 1.42876 0.25 0.380313 1.37747 0.25 0.405474 1.42126 0.25 -0.626221 1.23863 0.25 -0.642208 1.2262 0.25 -0.694803 1.24785 0.25 0.414972 1.41737 0.25 -0.657941 1.21347 0.25 -0.702645 1.24123 0.25 -0.710423 1.23454 0.25 -0.650106 1.21987 0.25 -0.718137 1.22777 0.25 -0.66571 1.20699 0.25 0.408395 1.36587 0.25 0.426911 1.35769 0.25 0.471178 1.39217 0.25 -0.673415 1.20043 0.25 -0.740885 1.20705 0.25 -0.688626 1.1871 0.25 0.436105 1.35346 0.25 0.454362 1.34475 0.25 0.498715 1.37838 0.25 -0.696132 1.18033 0.25 -0.710941 1.16656 0.25 -0.763033 1.18569 0.25 0.463423 1.34026 0.25 0.525837 1.36382 0.25 0.481409 1.33101 0.25 -0.718244 1.15957 0.25 -0.784568 1.1637 0.25 -0.732641 1.14537 0.25 0.490332 1.32626 0.25 0.508035 1.31649 0.25 0.552527 1.34851 0.25 -0.739736 1.13817 0.25 -0.753713 1.12356 0.25 -0.805477 1.14111 0.25 0.534223 1.30119 0.25 0.570072 1.33788 0.25 -0.760594 1.11615 0.25 0.587414 1.32692 0.25 0.542852 1.29592 0.25 0.578769 1.33244 0.25 -0.832357 1.11006 0.25 -0.838894 1.10214 0.25 -0.787396 1.08583 0.25 -0.780806 1.09351 0.25 -0.793913 1.07809 0.25 0.568429 1.27961 0.25 0.604547 1.31563 0.25 0.57685 1.274 0.25 0.585216 1.26831 0.25 0.621467 1.30402 0.25 0.613034 1.30986 0.25 0.593529 1.26254 0.25 0.629845 1.29809 0.25 -0.858039 1.07801 0.25 -0.806722 1.0624 0.25 0.638169 1.29208 0.25 0.601786 1.25669 0.25 -0.813013 1.05447 0.25 -0.864265 1.06986 0.25 0.654648 1.27983 0.25 0.646437 1.28599 0.25 -0.870413 1.06164 0.25 -0.819225 1.04647 0.25 0.609988 1.25075 0.25 0.662803 1.27358 0.25 -0.825356 1.03841 0.25 0.618133 1.24473 0.25 0.670896 1.26726 0.25 -0.876482 1.05337 0.25 -0.831407 1.0303 0.25 0.62622 1.23863 0.25 0.678928 1.26086 0.25 -0.837377 1.02212 0.25 -0.882472 1.04504 0.25 0.634246 1.23245 0.25 0.686897 1.25439 0.25 -0.888382 1.03666 0.25 -0.843265 1.0139 0.25 0.694803 1.24785 0.25 0.642208 1.2262 0.25 -0.849071 1.00561 0.25 -0.894213 1.02822 0.25 0.702645 1.24123 0.25 0.650106 1.21987 0.25 -0.899963 1.01972 0.25 -0.854795 0.997273 0.25 0.710423 1.23454 0.25 0.657941 1.21347 0.25 -0.905632 1.01118 0.25 -0.860436 0.98888 0.25 0.718137 1.22777 0.25 0.66571 1.20699 0.25 -0.865994 0.980433 0.25 -0.91122 1.00257 0.25 0.673415 1.20043 0.25 0.725785 1.22093 0.25 -0.916727 0.993921 0.25 -0.871469 0.971932 0.25 0.681053 1.1938 0.25 0.733368 1.21403 0.25 -0.876859 0.96338 0.25 -0.922152 0.985214 0.25 0.688626 1.1871 0.25 0.740885 1.20705 0.25 -0.927495 0.976456 0.25 -0.882166 0.954775 0.25 0.696132 1.18033 0.25 0.748335 1.2 0.25 -0.932755 0.967647 0.25 -0.887389 0.946118 0.25 0.70357 1.17348 0.25 0.755718 1.19288 0.25 -0.892526 0.937412 0.25 -0.937932 0.958787 0.25 0.710941 1.16656 0.25 0.763033 1.18569 0.25 -0.943026 0.949877 0.25 -0.897578 0.928655 0.25 0.77028 1.17843 0.25 0.718243 1.15957 0.25 -0.948036 0.940918 0.25 -0.902544 0.919848 0.25 -0.952961 0.93191 0.25 -0.907425 0.910993 0.25 -0.957802 0.922854 0.25 -0.912218 0.902089 0.25 0.739736 1.13817 0.25 0.791608 1.15624 0.25 0.784568 1.1637 0.25 -0.962558 0.913751 0.25 -0.916926 0.893138 0.25 -0.967228 0.904603 0.25 -0.921545 0.88414 0.25 -0.971809 0.895413 0.25 -0.926078 0.875096 0.25 -0.976302 0.886183 0.25 -0.930522 0.866006 0.25 -0.980707 0.876912 0.25 -0.934879 0.856871 0.25 -0.985022 0.867601 0.25 -0.989249 0.858251 0.25 -0.939146 0.847691 0.25 -0.993387 0.848863 0.25 -0.943325 0.838468 0.25 0.838894 1.10214 0.25 0.787396 1.08583 0.25 0.793913 1.07809 0.25 -0.947414 0.829201 0.25 -0.997435 0.839437 0.25 -0.951412 0.819895 0.25 -1.00139 0.829974 0.25 -0.955318 0.810553 0.25 -1.00526 0.820473 0.25 -0.95913 0.801175 0.25 -1.00904 0.810938 0.25 -1.01273 0.801366 0.25 -0.96285 0.791763 0.25 -1.01632 0.791761 0.25 -0.966477 0.782317 0.25 -1.01983 0.782121 0.25 -0.97001 0.772837 0.25 -1.02325 0.772448 0.25 -0.97345 0.763325 0.25 -1.02657 0.762742 0.25 -0.976796 0.753782 0.25 -1.0298 0.753004 0.25 -1.03294 0.743235 0.25 -0.980048 0.744207 0.25 0.905632 1.01118 0.25 0.854795 0.997273 0.25 0.860436 0.98888 0.25 -0.983206 0.734602 0.25 -1.03599 0.733434 0.25 -0.98627 0.724967 0.25 -1.03894 0.723604 0.25 -0.989239 0.715303 0.25 -1.0418 0.713745 0.25 -0.992113 0.705611 0.25 -1.04457 0.703856 0.25 -0.994893 0.695891 0.25 -1.04725 0.69394 0.25 -0.997577 0.686144 0.25 -1.04983 0.683999 0.25 -1.00017 0.676371 0.25 -1.05232 0.674037 0.25 -1.00266 0.666572 0.25 -1.05471 0.664053 0.25 -1.00506 0.656749 0.25 -1.05701 0.654049 0.25 -1.00736 0.646901 0.25 -1.05921 0.644026 0.25 -1.00956 0.637031 0.25 -1.06131 0.633983 0.25 -1.01167 0.627137 0.25 -1.06332 0.623922 0.25 -1.01368 0.617221 0.25 -1.0156 0.607284 0.25 -1.06524 0.613844 0.25 -1.06706 0.603749 0.25 -1.01742 0.597327 0.25 -1.06878 0.593637 0.25 -1.01914 0.587349 0.25 -1.02076 0.577352 0.25 -1.07041 0.58351 0.25 -1.07194 0.573368 0.25 -1.02229 0.567339 0.25 -1.07338 0.563212 0.25 -1.07472 0.553042 0.25 -1.02372 0.557313 0.25 -1.07596 0.542859 0.25 -1.02505 0.547277 0.25 -1.02628 0.53723 0.25 -1.07711 0.532665 0.25 -1.07816 0.522458 0.25 -1.02741 0.527175 0.25 -1.07911 0.512242 0.25 -1.02844 0.517111 0.25 -1.02937 0.507038 0.25 -1.07997 0.502015 0.25 -1.08073 0.491778 0.25 -1.03021 0.496959 0.25 -1.08139 0.481533 0.25 -1.03095 0.486874 0.25 -1.03158 0.476782 0.25 -1.08196 0.47128 0.25 -1.08242 0.46102 0.25 -1.03212 0.466686 0.25 -1.0828 0.450756 0.25 -1.03256 0.456586 0.25 -1.0329 0.446482 0.25 -1.08307 0.440491 0.25 -1.08325 0.430227 0.25 -1.03315 0.436376 0.25 -1.08333 0.419963 0.25 -1.03329 0.426267 0.25 -1.03333 0.416157 0.25 -1.08331 0.4097 0.25 -1.0832 0.39944 0.25 -1.03328 0.406047 0.25 -1.08298 0.389182 0.25 -1.03313 0.395937 0.25 -1.03252 0.37572 0.25 -1.03287 0.385827 0.25 -1.08268 0.378928 0.25 -1.08227 0.368678 0.25 -1.03207 0.365614 0.25 -1.03152 0.355512 0.25 -1.08177 0.358433 0.25 -1.08117 0.348194 0.25 -1.03087 0.345414 0.25 -1.08047 0.33796 0.25 -1.03013 0.33532 0.25 -1.02928 0.325232 0.25 -1.07968 0.327734 0.25 -1.07879 0.317515 0.25 -1.02833 0.315149 0.25 -1.0778 0.307304 0.25 -1.02729 0.305074 0.25 -1.02615 0.295011 0.25 -1.07672 0.297103 0.25 -1.07553 0.286911 0.25 -1.02491 0.284962 0.25 -1.07426 0.27673 0.25 -1.02357 0.274929 0.25 -1.02213 0.264912 0.25 -1.07288 0.266559 0.25 -1.07141 0.256401 0.25 -1.02059 0.254911 0.25 -1.06985 0.246255 0.25 -1.01896 0.244928 0.25 -1.01723 0.234963 0.25 -1.06818 0.236122 0.25 -1.01541 0.225016 0.25 -1.06642 0.226003 0.25 -1.06457 0.215901 0.25 -1.01348 0.215089 0.25 -1.06262 0.205819 0.25 -1.01146 0.205182 0.25 1.07997 0.502014 0.25 1.07911 0.512241 0.25 1.02937 0.507039 0.25 -1.00934 0.195296 0.25 -1.06057 0.195759 0.25 1.03021 0.496959 0.25 1.03095 0.486874 0.25 1.08073 0.491778 0.25 -1.05843 0.185721 0.25 1.08196 0.471279 0.25 1.08139 0.481533 0.25 1.03158 0.476783 0.25 -1.00713 0.185432 0.25 1.03212 0.466686 0.25 1.03256 0.456586 0.25 1.08242 0.461019 0.25 -1.0562 0.175704 0.25 1.08307 0.440491 0.25 1.0828 0.450756 0.25 1.0329 0.446482 0.25 -1.00482 0.17559 0.25 1.03315 0.436376 0.25 1.03329 0.426267 0.25 1.08325 0.430226 0.25 -1.05387 0.165711 0.25 1.08331 0.4097 0.25 1.08333 0.419963 0.25 1.03333 0.416157 0.25 -1.00241 0.165771 0.25 1.03328 0.406047 0.25 1.03313 0.395937 0.25 1.0832 0.39944 0.25 -1.05144 0.155742 0.25 1.08268 0.378928 0.25 1.08298 0.389182 0.25 1.03287 0.385827 0.25 -0.999909 0.155976 0.25 1.03252 0.37572 0.25 1.03207 0.365615 0.25 1.08227 0.368678 0.25 -1.04892 0.145797 0.25 1.08117 0.348193 0.25 1.08177 0.358433 0.25 1.03152 0.355512 0.25 -0.997311 0.146206 0.25 1.03087 0.345414 0.25 1.03013 0.33532 0.25 1.08047 0.33796 0.25 -1.04631 0.135877 0.25 1.07879 0.317515 0.25 1.07968 0.327734 0.25 1.02928 0.325232 0.25 -0.994617 0.13646 0.25 1.02833 0.315149 0.25 1.02729 0.305075 0.25 1.0778 0.307304 0.25 -1.04361 0.125983 0.25 1.07553 0.286911 0.25 1.07672 0.297103 0.25 1.02615 0.295011 0.25 -0.991827 0.126741 0.25 1.02491 0.284962 0.25 1.02357 0.274929 0.25 1.07426 0.27673 0.25 -1.04081 0.116115 0.25 1.07141 0.256401 0.25 1.07288 0.266559 0.25 1.02213 0.264912 0.25 -0.988942 0.117047 0.25 1.02059 0.254911 0.25 1.01896 0.244928 0.25 1.06985 0.246254 0.25 -1.03791 0.106274 0.25 1.06642 0.226002 0.25 1.06818 0.236121 0.25 1.01723 0.234963 0.25 -0.985962 0.107382 0.25 1.01541 0.225016 0.25 1.01348 0.215089 0.25 1.06457 0.215901 0.25 -1.03493 0.0964608 0.25 1.06057 0.195759 0.25 1.06262 0.205819 0.25 1.01146 0.205182 0.25 -0.982887 0.097744 0.25 1.00934 0.195297 0.25 1.05843 0.18572 0.25 -1.03185 0.086676 0.25 1.0562 0.175704 0.25 1.00713 0.185432 0.25 1.00482 0.17559 0.25 -0.979717 0.0881349 0.25 1.00241 0.165772 0.25 1.05144 0.155742 0.25 1.05387 0.165711 0.25 -1.02868 0.0769204 0.25 1.04892 0.145797 0.25 0.999909 0.155976 0.25 0.997311 0.146206 0.25 -0.976452 0.0785552 0.25 0.994617 0.13646 0.25 1.04361 0.125983 0.25 1.04631 0.135877 0.25 -1.02542 0.0671944 0.25 1.04081 0.116115 0.25 0.991827 0.126741 0.25 0.988942 0.117048 0.25 -0.973093 0.0690057 0.25 0.985962 0.107382 0.25 1.03493 0.0964607 0.25 1.03791 0.106274 0.25 -1.02206 0.0574988 0.25 1.03185 0.086676 0.25 0.982887 0.097744 0.25 0.979717 0.088135 0.25 -0.969639 0.0594872 0.25 -1.01861 0.0478342 0.25 -0.966092 0.05 0.25 -1.01507 0.0382013 0.25 1.01861 0.0478341 0.25 0.966092 0.05 0.25 1.01507 0.0382012 0.25 -1.01144 0.0286008 0.25 1.01144 0.0286007 0.25 -1.00772 0.0190333 0.25 1.00772 0.0190331 0.25 -1.00391 0.00949949 0.25 1.00391 0.00949949 0.25 -1 5.55112e-17 0.25 1 3.33067e-16 0.25 0.973093 0.0690059 0.25 0.969639 0.0594872 0.25 1.02206 0.0574986 0.25 1.02868 0.0769203 0.25 0.976452 0.0785554 0.25 1.02542 0.0671943 0.25 1.02844 0.517111 0.25 1.07816 0.522458 0.25 1.02741 0.527175 0.25 1.07711 0.532664 0.25 1.07596 0.542859 0.25 1.02628 0.537231 0.25 1.07472 0.553042 0.25 1.02505 0.547277 0.25 1.07338 0.563211 0.25 1.02372 0.557313 0.25 1.07194 0.573368 0.25 1.02229 0.567339 0.25 1.07041 0.58351 0.25 1.02076 0.577353 0.25 1.06878 0.593637 0.25 1.01914 0.587349 0.25 1.06706 0.603748 0.25 1.01742 0.597327 0.25 1.06524 0.613844 0.25 1.0156 0.607284 0.25 1.06332 0.623922 0.25 1.01368 0.617221 0.25 1.01167 0.627137 0.25 1.06131 0.633983 0.25 1.00956 0.637031 0.25 1.05921 0.644026 0.25 1.00736 0.646902 0.25 1.05701 0.654049 0.25 1.00506 0.656749 0.25 1.05471 0.664053 0.25 1.00266 0.666572 0.25 1.05232 0.674036 0.25 1.00017 0.676371 0.25 1.04983 0.683999 0.25 0.997577 0.686144 0.25 1.04725 0.693939 0.25 0.994893 0.695891 0.25 0.992113 0.705611 0.25 1.04457 0.703856 0.25 0.989239 0.715303 0.25 1.0418 0.713745 0.25 0.98627 0.724967 0.25 1.03894 0.723604 0.25 1.03599 0.733434 0.25 0.983206 0.734602 0.25 1.03294 0.743234 0.25 0.980048 0.744207 0.25 1.0298 0.753004 0.25 0.976796 0.753782 0.25 1.02657 0.762742 0.25 0.97345 0.763326 0.25 1.02325 0.772448 0.25 0.97001 0.772838 0.25 1.01983 0.782121 0.25 0.966477 0.782317 0.25 1.01632 0.79176 0.25 0.96285 0.791763 0.25 1.01273 0.801366 0.25 0.95913 0.801175 0.25 1.00904 0.810938 0.25 0.955317 0.810553 0.25 1.00526 0.820473 0.25 0.951412 0.819895 0.25 1.00139 0.829973 0.25 0.947414 0.829201 0.25 0.943325 0.838468 0.25 0.997435 0.839437 0.25 0.939146 0.847691 0.25 0.993387 0.848863 0.25 0.989249 0.858251 0.25 0.985022 0.867601 0.25 0.934879 0.856871 0.25 0.980707 0.876912 0.25 0.930522 0.866006 0.25 0.976302 0.886183 0.25 0.926078 0.875096 0.25 0.971809 0.895413 0.25 0.921545 0.88414 0.25 0.967228 0.904602 0.25 0.916925 0.893138 0.25 0.962559 0.91375 0.25 0.912218 0.90209 0.25 0.957803 0.922854 0.25 0.907424 0.910993 0.25 0.952961 0.93191 0.25 0.902544 0.919848 0.25 0.948036 0.940918 0.25 0.897578 0.928655 0.25 0.943026 0.949877 0.25 0.892526 0.937412 0.25 0.937932 0.958787 0.25 0.887388 0.946119 0.25 0.932755 0.967647 0.25 0.882166 0.954775 0.25 0.927495 0.976456 0.25 0.876859 0.96338 0.25 0.922152 0.985214 0.25 0.871469 0.971932 0.25 0.916727 0.99392 0.25 0.865994 0.980433 0.25 0.91122 1.00257 0.25 0.849071 1.00561 0.25 0.899963 1.01972 0.25 0.894213 1.02822 0.25 0.843265 1.0139 0.25 0.837377 1.02212 0.25 0.888382 1.03666 0.25 0.831407 1.0303 0.25 0.882472 1.04504 0.25 0.876482 1.05337 0.25 0.825356 1.03841 0.25 0.870413 1.06164 0.25 0.819225 1.04647 0.25 0.813012 1.05447 0.25 0.864265 1.06986 0.25 0.806722 1.0624 0.25 0.858039 1.07801 0.25 0.800355 1.07028 0.25 0.851735 1.08611 0.25 0.845353 1.09415 0.25 0.832358 1.11006 0.25 0.780805 1.09351 0.25 0.825746 1.11792 0.25 0.774141 1.10112 0.25 0.819062 1.12571 0.25 0.767404 1.10867 0.25 0.812305 1.13344 0.25 0.760594 1.11615 0.25 0.753713 1.12356 0.25 0.805477 1.14111 0.25 0.798578 1.14871 0.25 0.74676 1.1309 0.25 0.732641 1.14537 0.25 0.777459 1.1711 0.25 0.725477 1.15251 -0.25 -1.01144 0.0286008 -0.25 -1.01507 0.0382013 -0.25 -0.966092 0.05 -0.25 -0.561324 1.34323 -0.25 -0.525543 1.30638 -0.25 -0.570072 1.33788 -0.25 -1.02206 0.0574988 -0.25 -0.973093 0.0690057 -0.25 -0.969639 0.0594872 -0.25 1.05843 0.18572 -0.25 1.00934 0.195297 -0.25 1.06057 0.195759 -0.25 -1.02868 0.0769204 -0.25 -0.976452 0.0785552 -0.25 -1.02542 0.0671944 -0.25 1.06642 0.226002 -0.25 1.06457 0.215901 -0.25 1.01541 0.225016 -0.25 -0.982887 0.097744 -0.25 -0.979717 0.0881349 -0.25 -1.03185 0.086676 -0.25 1.01896 0.244928 -0.25 1.02059 0.254911 -0.25 1.06985 0.246254 -0.25 -0.985962 0.107382 -0.25 -1.03493 0.0964608 -0.25 -1.03791 0.106274 -0.25 1.07426 0.27673 -0.25 1.02357 0.274929 -0.25 1.02491 0.284962 -0.25 -0.988942 0.117047 -0.25 -1.04081 0.116115 -0.25 -0.991827 0.126741 -0.25 1.02833 0.315149 -0.25 1.0778 0.307304 -0.25 1.02729 0.305075 -0.25 -1.04361 0.125983 -0.25 -1.04631 0.135877 -0.25 -0.994617 0.13646 -0.25 1.03013 0.33532 -0.25 1.03087 0.345414 -0.25 1.08047 0.33796 -0.25 -1.04892 0.145797 -0.25 -0.999909 0.155976 -0.25 -0.997311 0.146206 -0.25 1.03252 0.37572 -0.25 1.08227 0.368678 -0.25 1.03207 0.365615 -0.25 -1.05387 0.165711 -0.25 -1.00241 0.165771 -0.25 -1.05144 0.155742 -0.25 1.03313 0.395937 -0.25 1.03328 0.406047 -0.25 1.0832 0.39944 -0.25 -1.00713 0.185432 -0.25 -1.00482 0.17559 -0.25 -1.0562 0.175704 -0.25 1.08307 0.440491 -0.25 1.08325 0.430226 -0.25 1.03315 0.436376 -0.25 -1.00934 0.195296 -0.25 -1.05843 0.185721 -0.25 -1.06057 0.195759 -0.25 1.03212 0.466686 -0.25 1.08196 0.471279 -0.25 1.08242 0.461019 -0.25 -1.06457 0.215901 -0.25 -1.01541 0.225016 -0.25 -1.01348 0.215089 -0.25 1.08073 0.491778 -0.25 1.03021 0.496959 -0.25 1.07997 0.502014 -0.25 -1.01896 0.244928 -0.25 -1.06985 0.246255 -0.25 -1.02059 0.254911 -0.25 -1.06818 0.236122 -0.25 -1.02491 0.284962 -0.25 -1.02357 0.274929 -0.25 -1.07426 0.27673 -0.25 -1.07288 0.266559 -0.25 -1.0778 0.307304 -0.25 -1.02833 0.315149 -0.25 -1.02729 0.305074 -0.25 -1.07672 0.297103 -0.25 -1.03013 0.33532 -0.25 -1.08047 0.33796 -0.25 -1.03087 0.345414 -0.25 -1.07968 0.327734 -0.25 -1.03252 0.37572 -0.25 -1.03207 0.365614 -0.25 -1.08227 0.368678 -0.25 -1.08177 0.358433 -0.25 -1.08331 0.4097 -0.25 -1.03328 0.406047 -0.25 -1.0832 0.39944 -0.25 1.08177 0.358433 -0.25 -1.08325 0.430227 -0.25 -1.08307 0.440491 -0.25 -1.03315 0.436376 -0.25 -1.03333 0.416157 -0.25 -1.08333 0.419963 -0.25 -1.03329 0.426267 -0.25 -1.03212 0.466686 -0.25 -1.08242 0.46102 -0.25 -1.08196 0.47128 -0.25 -1.03256 0.456586 -0.25 -1.0329 0.446482 -0.25 -1.0828 0.450756 -0.25 -1.07997 0.502015 -0.25 -1.03021 0.496959 -0.25 -1.08073 0.491778 -0.25 -1.08139 0.481533 -0.25 -1.03095 0.486874 -0.25 -1.03158 0.476782 -0.25 -1.07816 0.522458 -0.25 -1.07711 0.532665 -0.25 -1.02741 0.527175 -0.25 -1.02937 0.507038 -0.25 -1.07911 0.512242 -0.25 -1.02844 0.517111 -0.25 -1.02628 0.53723 -0.25 -1.07596 0.542859 -0.25 -1.02505 0.547277 -0.25 -1.02229 0.567339 -0.25 -1.07194 0.573368 -0.25 -1.02076 0.577352 -0.25 -1.02372 0.557313 -0.25 -1.07472 0.553042 -0.25 -1.07338 0.563212 -0.25 -1.06706 0.603749 -0.25 -1.06524 0.613844 -0.25 -1.0156 0.607284 -0.25 -1.07041 0.58351 -0.25 -1.01914 0.587349 -0.25 -1.00956 0.637031 -0.25 -1.06131 0.633983 -0.25 -1.05921 0.644026 -0.25 -1.01368 0.617221 -0.25 -1.05232 0.674037 -0.25 -1.00266 0.666572 -0.25 -1.05471 0.664053 -0.25 -1.00736 0.646901 -0.25 -1.04725 0.69394 -0.25 -1.04457 0.703856 -0.25 -0.994893 0.695891 -0.25 -1.00017 0.676371 -0.25 -0.98627 0.724967 -0.25 -1.03894 0.723604 -0.25 -1.03599 0.733434 -0.25 -0.992113 0.705611 -0.25 -0.980048 0.744207 -0.25 -1.03294 0.743235 -0.25 -1.0298 0.753004 -0.25 -0.983206 0.734602 -0.25 -0.97001 0.772837 -0.25 -1.01983 0.782121 -0.25 -0.966477 0.782317 -0.25 -1.02657 0.762742 -0.25 -0.97345 0.763325 -0.25 -0.976796 0.753782 -0.25 -0.95913 0.801175 -0.25 -1.00904 0.810938 -0.25 -1.00526 0.820473 -0.25 -1.01632 0.791761 -0.25 -0.993387 0.848863 -0.25 -0.947414 0.829201 -0.25 -0.997435 0.839437 -0.25 -0.955318 0.810553 -0.25 -0.985022 0.867601 -0.25 -0.939146 0.847691 -0.25 -0.989249 0.858251 -0.25 -0.943325 0.838468 -0.25 -0.921545 0.88414 -0.25 -0.926078 0.875096 -0.25 -0.971809 0.895413 -0.25 -0.934879 0.856871 -0.25 -0.980707 0.876912 -0.25 0.791608 1.15624 -0.25 0.739736 1.13817 -0.25 0.784568 1.1637 -0.25 -0.967228 0.904603 -0.25 0.718243 1.15957 -0.25 0.763033 1.18569 -0.25 0.77028 1.17843 -0.25 -0.957802 0.922854 -0.25 -0.952961 0.93191 -0.25 -0.907425 0.910993 -0.25 0.740885 1.20705 -0.25 0.748335 1.2 -0.25 0.696132 1.18033 -0.25 -0.897578 0.928655 -0.25 -0.943026 0.949877 -0.25 -0.892526 0.937412 -0.25 0.673415 1.20043 -0.25 0.718137 1.22777 -0.25 0.725785 1.22093 -0.25 -0.876859 0.96338 -0.25 -0.882166 0.954775 -0.25 -0.927495 0.976456 -0.25 0.650106 1.21987 -0.25 0.694803 1.24785 -0.25 0.702645 1.24123 -0.25 -0.865994 0.980433 -0.25 -0.91122 1.00257 -0.25 -0.860436 0.98888 -0.25 0.62622 1.23863 -0.25 0.670896 1.26726 -0.25 0.678928 1.26086 -0.25 -0.843265 1.0139 -0.25 -0.849071 1.00561 -0.25 -0.894213 1.02822 -0.25 0.646437 1.28599 -0.25 0.654648 1.27983 -0.25 0.601786 1.25669 -0.25 -0.831407 1.0303 -0.25 -0.876482 1.05337 -0.25 -0.825356 1.03841 -0.25 0.585216 1.26831 -0.25 0.57685 1.274 -0.25 0.621467 1.30402 -0.25 -0.813013 1.05447 -0.25 -0.864265 1.06986 -0.25 -0.858039 1.07801 -0.25 0.559956 1.28513 -0.25 0.55143 1.29057 -0.25 0.596007 1.32131 -0.25 -0.845353 1.09415 -0.25 -0.838894 1.10214 -0.25 -0.793913 1.07809 -0.25 0.570072 1.33788 -0.25 0.534223 1.30119 -0.25 0.525543 1.30638 -0.25 -0.819062 1.12571 -0.25 -0.774141 1.10112 -0.25 -0.825746 1.11792 -0.25 0.552527 1.34851 -0.25 0.508035 1.31649 -0.25 0.543679 1.3537 -0.25 -0.753713 1.12356 -0.25 -0.805477 1.14111 -0.25 -0.798578 1.14871 -0.25 0.481409 1.33101 -0.25 0.516844 1.36876 -0.25 0.525837 1.36382 -0.25 -0.777459 1.1711 -0.25 -0.732641 1.14537 -0.25 -0.784568 1.1637 -0.25 0.489582 1.38306 -0.25 0.498715 1.37838 -0.25 0.454362 1.34475 -0.25 -0.710941 1.16656 -0.25 -0.763033 1.18569 -0.25 -0.755718 1.19288 -0.25 0.471178 1.39217 -0.25 0.426911 1.35769 -0.25 0.461909 1.39659 -0.25 -0.740885 1.20705 -0.25 -0.733368 1.21403 -0.25 -0.688626 1.1871 -0.25 0.399074 1.36983 -0.25 0.433859 1.40933 -0.25 0.443247 1.40517 -0.25 -0.710423 1.23454 -0.25 -0.66571 1.20699 -0.25 -0.718137 1.22777 -0.25 0.37088 1.38115 -0.25 0.405474 1.42126 -0.25 0.380313 1.37747 -0.25 -0.642208 1.2262 -0.25 -0.694803 1.24785 -0.25 -0.686897 1.25439 -0.25 0.376774 1.43237 -0.25 0.351915 1.38823 -0.25 0.342385 1.39163 -0.25 -0.670896 1.26726 -0.25 -0.662802 1.27358 -0.25 -0.618133 1.24473 -0.25 0.323232 1.39814 -0.25 0.313611 1.40126 -0.25 0.347774 1.44266 -0.25 -0.601786 1.25669 -0.25 -0.646436 1.28599 -0.25 -0.638169 1.29208 -0.25 0.284577 1.41004 -0.25 0.318494 1.45212 -0.25 0.294282 1.40721 -0.25 -0.621467 1.30402 -0.25 -0.613034 1.30986 -0.25 -0.57685 1.274 -0.25 0.255302 1.41796 -0.25 0.288951 1.46075 -0.25 0.265086 1.41542 -0.25 -0.596007 1.32131 -0.25 -0.587414 1.32692 -0.25 -0.55143 1.29057 -0.25 0.235662 1.42277 -0.25 0.259163 1.46854 -0.25 0.245494 1.42041 -0.25 0.215928 1.42719 -0.25 0.206029 1.42925 -0.25 0.229148 1.47549 -0.25 -0.543679 1.3537 -0.25 -0.534782 1.3588 -0.25 -0.499207 1.32142 -0.25 0.176205 1.43487 -0.25 0.198954 1.48157 -0.25 0.186166 1.43309 -0.25 -0.507803 1.37361 -0.25 -0.472439 1.33568 -0.25 -0.516843 1.36876 -0.25 0.146211 1.4396 -0.25 0.168616 1.4868 -0.25 0.156227 1.43812 -0.25 -0.480402 1.38766 -0.25 -0.445256 1.34915 -0.25 -0.489582 1.38306 -0.25 0.126131 1.44227 -0.25 0.127972 1.49241 -0.25 0.138152 1.49116 -0.25 -0.417675 1.36183 -0.25 -0.461909 1.39659 -0.25 -0.452598 1.40093 -0.25 0.0959358 1.44554 -0.25 0.0973683 1.49562 -0.25 0.10758 1.49465 -0.25 -0.389713 1.37369 -0.25 -0.433859 1.40933 -0.25 -0.424433 1.41339 -0.25 0.0656781 1.44791 -0.25 0.0666821 1.49795 -0.25 0.0769189 1.49727 -0.25 -0.37088 1.38115 -0.25 -0.395942 1.42505 -0.25 -0.361414 1.38474 -0.25 0.0353779 1.44939 -0.25 0.0359317 1.4994 -0.25 0.0461877 1.49901 -0.25 -0.332823 1.39493 -0.25 -0.342385 1.39163 -0.25 -0.367139 1.43589 -0.25 0.00505468 1.44999 -0.25 0.00513585 1.49999 -0.25 0.015405 1.49989 -0.25 -0.347774 1.44266 -0.25 -0.338045 1.44591 -0.25 -0.313611 1.40126 -0.25 -0.0151639 1.44989 -0.25 -0.0252719 1.44969 -0.25 -0.0256704 1.4997 -0.25 -0.284577 1.41004 -0.25 -0.318494 1.45212 -0.25 -0.308675 1.45509 -0.25 -0.0454816 1.449 -0.25 -0.056438 1.49853 -0.25 -0.0461877 1.49901 -0.25 -0.255303 1.41796 -0.25 -0.288951 1.46075 -0.25 -0.279048 1.46344 -0.25 -0.0871478 1.49649 -0.25 -0.0769187 1.49727 -0.25 -0.0757699 1.44722 -0.25 -0.225807 1.42503 -0.25 -0.235662 1.42277 -0.25 -0.249182 1.47095 -0.25 -0.106009 1.44455 -0.25 -0.117781 1.49358 -0.25 -0.10758 1.49465 -0.25 -0.196108 1.43122 -0.25 -0.206029 1.42925 -0.25 -0.219101 1.47761 -0.25 -0.156227 1.43812 -0.25 -0.168616 1.4868 -0.25 -0.146211 1.4396 -0.25 -0.188857 1.48341 -0.25 -0.166226 1.43654 -0.25 -0.176206 1.43487 -0.25 -0.148319 1.4898 -0.25 -0.126132 1.44227 -0.25 -0.136179 1.44099 -0.25 -0.158474 1.48835 -0.25 -0.178744 1.48515 -0.25 -0.138152 1.49116 -0.25 -0.116074 1.44346 -0.25 -0.186166 1.43309 -0.25 -0.198954 1.48157 -0.25 -0.127972 1.49241 -0.25 -0.209036 1.47964 -0.25 -0.0959358 1.44554 -0.25 -0.0973682 1.49562 -0.25 -0.215929 1.42719 -0.25 -0.229148 1.47549 -0.25 -0.085856 1.44643 -0.25 -0.0666819 1.49795 -0.25 -0.0656783 1.44791 -0.25 -0.239176 1.47327 -0.25 -0.269119 1.46604 -0.25 -0.259163 1.46854 -0.25 -0.0555819 1.4485 -0.25 -0.035378 1.44939 -0.25 -0.245494 1.42041 -0.25 -0.298827 1.45797 -0.25 -0.265086 1.41542 -0.25 -0.0359316 1.4994 -0.25 -0.00505475 1.44999 -0.25 -0.0154047 1.49989 -0.25 -0.274845 1.41278 -0.25 -0.328284 1.44906 -0.25 -0.294283 1.40721 -0.25 -0.00513546 1.49999 -0.25 0.0256706 1.4997 -0.25 0.0151637 1.44989 -0.25 -0.303961 1.40428 -0.25 -0.357473 1.43932 -0.25 -0.323232 1.39814 -0.25 0.0252717 1.44969 -0.25 0.0564381 1.49853 -0.25 0.0454815 1.449 -0.25 -0.351915 1.38823 -0.25 -0.376773 1.43237 -0.25 0.0555818 1.4485 -0.25 0.0871478 1.49649 -0.25 0.0757697 1.44722 -0.25 -0.386374 1.42876 -0.25 -0.380313 1.37747 -0.25 -0.405474 1.42126 -0.25 0.0858559 1.44643 -0.25 0.117781 1.49358 -0.25 0.106009 1.44455 -0.25 -0.414972 1.41737 -0.25 -0.399075 1.36983 -0.25 -0.443247 1.40517 -0.25 0.116074 1.44346 -0.25 0.148319 1.4898 -0.25 0.136178 1.44099 -0.25 -0.408395 1.36587 -0.25 -0.426912 1.35769 -0.25 -0.471178 1.39217 -0.25 0.158474 1.48835 -0.25 0.178744 1.48515 -0.25 0.166225 1.43654 -0.25 -0.436106 1.35346 -0.25 -0.454362 1.34475 -0.25 -0.498715 1.37838 -0.25 0.188857 1.48341 -0.25 0.209036 1.47964 -0.25 0.196108 1.43122 -0.25 -0.463423 1.34026 -0.25 -0.481409 1.33101 -0.25 -0.525837 1.36382 -0.25 0.219101 1.47761 -0.25 0.239177 1.47327 -0.25 0.225806 1.42503 -0.25 -0.490332 1.32626 -0.25 -0.508035 1.31649 -0.25 -0.552527 1.34851 -0.25 0.249182 1.47095 -0.25 0.269119 1.46604 -0.25 -0.516814 1.31147 -0.25 -0.578769 1.33244 -0.25 -0.534223 1.30119 -0.25 0.279048 1.46344 -0.25 0.298827 1.45797 -0.25 0.274845 1.41278 -0.25 -0.542852 1.29592 -0.25 -0.604547 1.31563 -0.25 -0.559956 1.28513 -0.25 0.308675 1.45509 -0.25 0.303961 1.40428 -0.25 0.328284 1.44906 -0.25 -0.568429 1.27961 -0.25 -0.585216 1.26831 -0.25 -0.629845 1.29809 -0.25 0.338045 1.44591 -0.25 0.357473 1.43932 -0.25 0.332823 1.39493 -0.25 -0.593529 1.26254 -0.25 -0.654648 1.27983 -0.25 0.367139 1.43589 -0.25 0.386375 1.42876 -0.25 0.361414 1.38474 -0.25 -0.609988 1.25075 -0.25 -0.626221 1.23863 -0.25 -0.678928 1.26086 -0.25 0.395942 1.42505 -0.25 0.414972 1.41737 -0.25 0.389712 1.37369 -0.25 -0.634246 1.23245 -0.25 -0.650106 1.21987 -0.25 -0.702645 1.24123 -0.25 0.424434 1.41339 -0.25 0.408395 1.36587 -0.25 0.452598 1.40093 -0.25 -0.657941 1.21347 -0.25 -0.725785 1.22093 -0.25 -0.673415 1.20043 -0.25 0.417675 1.36183 -0.25 0.480402 1.38766 -0.25 0.436105 1.35346 -0.25 -0.681054 1.1938 -0.25 -0.696132 1.18033 -0.25 -0.748335 1.2 -0.25 0.445256 1.34915 -0.25 0.463423 1.34026 -0.25 0.507803 1.37361 -0.25 -0.70357 1.17348 -0.25 -0.718244 1.15957 -0.25 -0.77028 1.17843 -0.25 0.472439 1.33568 -0.25 0.534782 1.3588 -0.25 0.490332 1.32626 -0.25 -0.725477 1.15251 -0.25 -0.791608 1.15624 -0.25 -0.739736 1.13817 -0.25 0.499207 1.32142 -0.25 0.516813 1.31147 -0.25 0.561325 1.34323 -0.25 -0.74676 1.1309 -0.25 -0.760594 1.11615 -0.25 -0.812305 1.13344 -0.25 0.578769 1.33244 -0.25 0.542852 1.29592 -0.25 -0.767404 1.10867 -0.25 -0.832357 1.11006 -0.25 -0.780806 1.09351 -0.25 0.587414 1.32692 -0.25 0.568429 1.27961 -0.25 0.604547 1.31563 -0.25 -0.787396 1.08583 -0.25 -0.800355 1.07028 -0.25 -0.851735 1.08611 -0.25 0.613034 1.30986 -0.25 0.629845 1.29809 -0.25 0.593529 1.26254 -0.25 -0.806722 1.0624 -0.25 -0.819225 1.04647 -0.25 -0.870413 1.06164 -0.25 0.638169 1.29208 -0.25 0.609988 1.25075 -0.25 0.662803 1.27358 -0.25 -0.837377 1.02212 -0.25 -0.882472 1.04504 -0.25 0.618133 1.24473 -0.25 0.634246 1.23245 -0.25 0.686897 1.25439 -0.25 -0.888382 1.03666 -0.25 -0.899963 1.01972 -0.25 -0.854795 0.997273 -0.25 0.642208 1.2262 -0.25 0.657941 1.21347 -0.25 0.710423 1.23454 -0.25 -0.905632 1.01118 -0.25 -0.871469 0.971932 -0.25 -0.916727 0.993921 -0.25 0.66571 1.20699 -0.25 0.681053 1.1938 -0.25 0.733368 1.21403 -0.25 -0.922152 0.985214 -0.25 0.688626 1.1871 -0.25 -0.937932 0.958787 -0.25 -0.887389 0.946118 -0.25 -0.932755 0.967647 -0.25 0.710941 1.16656 -0.25 0.755718 1.19288 -0.25 0.70357 1.17348 -0.25 -0.948036 0.940918 -0.25 -0.902544 0.919848 -0.25 -0.912218 0.902089 -0.25 -0.916926 0.893138 -0.25 -0.962558 0.913751 -0.25 -0.976302 0.886183 -0.25 -0.930522 0.866006 -0.25 0.838894 1.10214 -0.25 0.793913 1.07809 -0.25 0.787396 1.08583 -0.25 -1.00139 0.829974 -0.25 -0.951412 0.819895 -0.25 -1.01273 0.801366 -0.25 -0.96285 0.791763 -0.25 -1.02325 0.772448 -0.25 0.854795 0.997273 -0.25 0.905632 1.01118 -0.25 0.860436 0.98888 -0.25 -1.0418 0.713745 -0.25 -0.989239 0.715303 -0.25 -0.997577 0.686144 -0.25 -1.04983 0.683999 -0.25 -1.05701 0.654049 -0.25 -1.00506 0.656749 -0.25 -1.06332 0.623922 -0.25 -1.01167 0.627137 -0.25 -1.01742 0.597327 -0.25 -1.06878 0.593637 -0.25 -1.03313 0.395937 -0.25 -1.08268 0.378928 -0.25 -1.03287 0.385827 -0.25 -1.08298 0.389182 -0.25 -1.08117 0.348194 -0.25 -1.03152 0.355512 -0.25 -1.02928 0.325232 -0.25 -1.07879 0.317515 -0.25 -1.07553 0.286911 -0.25 -1.02615 0.295011 -0.25 -1.07141 0.256401 -0.25 -1.02213 0.264912 -0.25 -1.01723 0.234963 -0.25 -1.06642 0.226003 -0.25 -1.06262 0.205819 -0.25 -1.01146 0.205182 -0.25 1.00713 0.185432 -0.25 1.08139 0.481533 -0.25 1.03158 0.476783 -0.25 1.03095 0.486874 -0.25 1.0329 0.446482 -0.25 1.0828 0.450756 -0.25 1.03256 0.456586 -0.25 1.03333 0.416157 -0.25 1.08331 0.4097 -0.25 1.08333 0.419963 -0.25 1.03329 0.426267 -0.25 1.08268 0.378928 -0.25 1.03287 0.385827 -0.25 1.08298 0.389182 -0.25 1.03152 0.355512 -0.25 1.08117 0.348193 -0.25 1.07879 0.317515 -0.25 1.02928 0.325232 -0.25 1.07968 0.327734 -0.25 1.02615 0.295011 -0.25 1.07553 0.286911 -0.25 1.07672 0.297103 -0.25 1.02213 0.264912 -0.25 1.07141 0.256401 -0.25 1.07288 0.266559 -0.25 1.01723 0.234963 -0.25 1.06818 0.236121 -0.25 1.01146 0.205182 -0.25 1.06262 0.205819 -0.25 1.01348 0.215089 -0.25 1.0562 0.175704 -0.25 -1.01861 0.0478342 -0.25 1.00241 0.165772 -0.25 1.05387 0.165711 -0.25 1.05144 0.155742 -0.25 1.00482 0.17559 -0.25 1.04892 0.145797 -0.25 0.999909 0.155976 -0.25 -1.00772 0.0190333 -0.25 -1.00391 0.00949949 -0.25 -1 5.55112e-17 -0.25 1 3.33067e-16 -0.25 1.07911 0.512241 -0.25 1.02937 0.507039 -0.25 1.02844 0.517111 -0.25 1.07816 0.522458 -0.25 1.02741 0.527175 -0.25 1.07711 0.532664 -0.25 1.02628 0.537231 -0.25 1.07596 0.542859 -0.25 1.02505 0.547277 -0.25 1.07472 0.553042 -0.25 1.02372 0.557313 -0.25 1.07338 0.563211 -0.25 1.02229 0.567339 -0.25 1.07194 0.573368 -0.25 1.02076 0.577353 -0.25 1.07041 0.58351 -0.25 1.01914 0.587349 -0.25 1.06878 0.593637 -0.25 1.01742 0.597327 -0.25 1.06706 0.603748 -0.25 1.0156 0.607284 -0.25 1.06524 0.613844 -0.25 1.01368 0.617221 -0.25 1.06332 0.623922 -0.25 1.01167 0.627137 -0.25 1.00956 0.637031 -0.25 1.06131 0.633983 -0.25 1.00736 0.646902 -0.25 1.05921 0.644026 -0.25 1.00506 0.656749 -0.25 1.05701 0.654049 -0.25 1.00266 0.666572 -0.25 1.05471 0.664053 -0.25 1.00017 0.676371 -0.25 1.05232 0.674036 -0.25 1.04983 0.683999 -0.25 0.997577 0.686144 -0.25 1.04725 0.693939 -0.25 0.994893 0.695891 -0.25 0.992113 0.705611 -0.25 1.04457 0.703856 -0.25 0.989239 0.715303 -0.25 1.0418 0.713745 -0.25 0.98627 0.724967 -0.25 1.03894 0.723604 -0.25 0.983206 0.734602 -0.25 1.03599 0.733434 -0.25 0.980048 0.744207 -0.25 1.03294 0.743234 -0.25 0.976796 0.753782 -0.25 1.0298 0.753004 -0.25 0.97345 0.763326 -0.25 1.02657 0.762742 -0.25 0.97001 0.772838 -0.25 1.02325 0.772448 -0.25 0.966477 0.782317 -0.25 1.01983 0.782121 -0.25 0.96285 0.791763 -0.25 1.01632 0.79176 -0.25 1.01273 0.801366 -0.25 0.95913 0.801175 -0.25 1.00904 0.810938 -0.25 0.955317 0.810553 -0.25 1.00526 0.820473 -0.25 0.951412 0.819895 -0.25 1.00139 0.829973 -0.25 0.947414 0.829201 -0.25 0.943325 0.838468 -0.25 0.997435 0.839437 -0.25 0.939146 0.847691 -0.25 0.993387 0.848863 -0.25 0.989249 0.858251 -0.25 0.985022 0.867601 -0.25 0.934879 0.856871 -0.25 0.980707 0.876912 -0.25 0.930522 0.866006 -0.25 0.976302 0.886183 -0.25 0.926078 0.875096 -0.25 0.921545 0.88414 -0.25 0.971809 0.895413 -0.25 0.967228 0.904602 -0.25 0.916925 0.893138 -0.25 0.962559 0.91375 -0.25 0.912218 0.90209 -0.25 0.957803 0.922854 -0.25 0.907424 0.910993 -0.25 0.952961 0.93191 -0.25 0.902544 0.919848 -0.25 0.897578 0.928655 -0.25 0.948036 0.940918 -0.25 0.892526 0.937412 -0.25 0.943026 0.949877 -0.25 0.887388 0.946119 -0.25 0.937932 0.958787 -0.25 0.882166 0.954775 -0.25 0.932755 0.967647 -0.25 0.927495 0.976456 -0.25 0.876859 0.96338 -0.25 0.922152 0.985214 -0.25 0.871469 0.971932 -0.25 0.916727 0.99392 -0.25 0.865994 0.980433 -0.25 0.91122 1.00257 -0.25 0.849071 1.00561 -0.25 0.899963 1.01972 -0.25 0.843265 1.0139 -0.25 0.894213 1.02822 -0.25 0.837377 1.02212 -0.25 0.888382 1.03666 -0.25 0.831407 1.0303 -0.25 0.882472 1.04504 -0.25 0.825356 1.03841 -0.25 0.876482 1.05337 -0.25 0.819225 1.04647 -0.25 0.870413 1.06164 -0.25 0.813012 1.05447 -0.25 0.864265 1.06986 -0.25 0.806722 1.0624 -0.25 0.858039 1.07801 -0.25 0.800355 1.07028 -0.25 0.851735 1.08611 -0.25 0.845353 1.09415 -0.25 0.780805 1.09351 -0.25 0.832358 1.11006 -0.25 0.825746 1.11792 -0.25 0.774141 1.10112 -0.25 0.819062 1.12571 -0.25 0.767404 1.10867 -0.25 0.812305 1.13344 -0.25 0.760594 1.11615 -0.25 0.753713 1.12356 -0.25 0.805477 1.14111 -0.25 0.798578 1.14871 -0.25 0.74676 1.1309 -0.25 0.732641 1.14537 -0.25 0.777459 1.1711 -0.25 0.725477 1.15251 -0.25 0.997311 0.146206 -0.25 1.04631 0.135877 -0.25 0.994617 0.13646 -0.25 1.04361 0.125983 -0.25 0.991827 0.126741 -0.25 0.988942 0.117048 -0.25 1.04081 0.116115 -0.25 1.03791 0.106274 -0.25 0.985962 0.107382 -0.25 1.03493 0.0964607 -0.25 0.982887 0.097744 -0.25 0.979717 0.088135 -0.25 1.03185 0.086676 -0.25 1.02868 0.0769203 -0.25 0.976452 0.0785554 -0.25 1.02542 0.0671943 -0.25 0.973093 0.0690059 -0.25 0.969639 0.0594872 -0.25 1.02206 0.0574986 -0.25 1.01861 0.0478341 -0.25 0.966092 0.05 -0.25 1.01144 0.0286007 -0.25 1.01507 0.0382012 -0.25 1.00772 0.0190331 -0.25 1.00391 0.00949949 0 -0.0047282 0.999989 0 -0.00473625 0.999989 0 0.0047283 0.999989 0 -0.0142086 0.999899 0 -0.0142084 0.999899 0 -0.0236794 0.99972 0 -0.0236796 0.99972 0 -0.0331484 0.99945 0 -0.0331483 0.99945 0 -0.0426143 0.999092 0 -0.0426142 0.999092 0 -0.0520761 0.998643 0 -0.0520764 0.998643 0 -0.0615337 0.998105 0 -0.0615335 0.998105 0 -0.0709855 0.997477 0 -0.0709854 0.997477 0 -0.0804309 0.99676 0 -0.080431 0.99676 0 -0.0898693 0.995954 0 -0.0898692 0.995954 0 -0.0992995 0.995058 0 -0.0992993 0.995058 0 -0.108721 0.994072 0 -0.108721 0.994072 0 -0.118132 0.992998 0 -0.118132 0.992998 0 -0.127533 0.991834 0 -0.127533 0.991834 0 -0.136923 0.990582 0 -0.136923 0.990582 0 -0.1463 0.98924 0 -0.1463 0.98924 0 -0.155664 0.98781 0 -0.155664 0.98781 0 -0.165014 0.986291 0 -0.165014 0.986291 0 -0.174349 0.984684 0 -0.174349 0.984684 0 -0.183669 0.982988 0 -0.183669 0.982988 0 -0.192972 0.981204 0 -0.192972 0.981204 0 -0.202258 0.979332 0 -0.202258 0.979332 0 -0.211526 0.977372 0 -0.211526 0.977372 0 -0.220774 0.975325 0 -0.220775 0.975325 0 -0.230003 0.97319 0 -0.230003 0.97319 0 -0.239212 0.970967 0 -0.239211 0.970967 0 -0.248398 0.968658 0 -0.248399 0.968658 0 -0.257563 0.966262 0 -0.257563 0.966262 0 -0.266704 0.963778 0 -0.266704 0.963778 0 -0.275822 0.961209 0 -0.275822 0.961209 0 -0.284915 0.958553 0 -0.284915 0.958553 0 -0.293982 0.955811 0 -0.293982 0.955811 0 -0.303022 0.952983 0 -0.303023 0.952983 0 -0.312036 0.95007 0 -0.312036 0.95007 0 -0.321022 0.947072 0 -0.321022 0.947072 0 -0.329979 0.943988 0 -0.329979 0.943988 0 -0.338906 0.94082 0 -0.338906 0.94082 0 -0.347803 0.937568 0 -0.347803 0.937568 0 -0.356668 0.934231 0 -0.356668 0.934231 0 -0.365502 0.930811 0 -0.365502 0.930811 0 -0.374303 0.927307 0 -0.374302 0.927307 0 -0.38307 0.923719 0 -0.38307 0.923719 0 -0.391803 0.920049 0 -0.391802 0.920049 0 -0.4005 0.916297 0 -0.4005 0.916297 0 -0.409162 0.912462 0 -0.409162 0.912462 0 -0.417787 0.908545 0 -0.417787 0.908545 0 -0.426374 0.904547 0 -0.426374 0.904547 0 -0.434924 0.900467 0 -0.434924 0.900467 0 -0.443434 0.896307 0 -0.443434 0.896307 0 -0.451904 0.892066 0 -0.451904 0.892066 0 -0.460334 0.887746 0 -0.460334 0.887746 0 -0.468723 0.883345 0 -0.468723 0.883345 0 -0.477069 0.878866 0 -0.477069 0.878866 0 -0.485373 0.874307 0 -0.485373 0.874307 0 -0.493633 0.86967 0 -0.493633 0.86967 0 -0.501849 0.864955 0 -0.501849 0.864955 0 -0.51002 0.860163 0 -0.51002 0.860163 0 -0.518145 0.855293 0 -0.518145 0.855293 0 -0.526224 0.850346 0 -0.526223 0.850346 0 -0.534255 0.845324 0 -0.534255 0.845323 0 -0.542238 0.840225 0 -0.542238 0.840225 0 -0.550173 0.835051 0 -0.550173 0.835051 0 -0.558058 0.829802 0 -0.558059 0.829802 0 -0.565894 0.824478 0 -0.565894 0.824478 0 -0.573678 0.819081 0 -0.573678 0.819081 0 -0.581411 0.81361 0 -0.581411 0.81361 0 -0.589092 0.808066 0 -0.589092 0.808066 0 -0.59672 0.802449 0 -0.59672 0.802449 0 -0.604295 0.796761 0 -0.604295 0.796761 0 -0.611815 0.791001 0 -0.611815 0.791001 0 -0.61928 0.78517 0 -0.61928 0.78517 0 -0.62669 0.779269 0 -0.62669 0.779268 0 -0.634044 0.773297 0 -0.634044 0.773297 0 -0.64134 0.767256 0 -0.64134 0.767256 0 -0.648579 0.761147 0 -0.64858 0.761147 0 -0.65576 0.754969 0 -0.65576 0.754969 0 -0.662883 0.748723 0 -0.662882 0.748724 0 -0.669945 0.742411 0 -0.669945 0.742411 0 -0.676948 0.736031 0 -0.676947 0.736031 0 -0.683889 0.729586 0 -0.683889 0.729586 0 -0.69077 0.723075 0 -0.69077 0.723075 0 -0.697588 0.716499 0 -0.697588 0.716499 0 -0.704344 0.709859 0 -0.704344 0.709859 0 -0.711036 0.703155 0 -0.711037 0.703155 0 -0.717665 0.696388 0 -0.717665 0.696388 0 -0.72423 0.689559 0 -0.72423 0.689559 0 -0.730729 0.682668 0 -0.730729 0.682668 0 -0.737163 0.675715 0 -0.737163 0.675715 0 -0.743531 0.668702 0 -0.743531 0.668702 0 -0.749832 0.661629 0 -0.749832 0.661629 0 -0.756065 0.654496 0 -0.756065 0.654496 0 -0.762231 0.647305 0 -0.762231 0.647305 0 -0.768328 0.640056 0 -0.768329 0.640056 0 -0.774357 0.632749 0 -0.774357 0.632749 0 -0.780316 0.625385 0 -0.780316 0.625385 0 -0.786205 0.617966 0 -0.786205 0.617966 0 -0.792024 0.610491 0 -0.792023 0.610491 0 -0.797771 0.602961 0 -0.797771 0.602961 0 -0.803447 0.595377 0 -0.803447 0.595377 0 -0.809051 0.587739 0 -0.80905 0.587739 0 -0.814582 0.580049 0 -0.814581 0.580049 0 -0.820039 0.572307 0 -0.82004 0.572307 0 -0.825424 0.564513 0 -0.825424 0.564513 0 -0.830734 0.556669 0 -0.830734 0.556669 0 -0.83597 0.548775 0 -0.83597 0.548775 0 -0.841131 0.540831 0 -0.841131 0.540832 0 -0.846216 0.532839 0 -0.846216 0.53284 0 -0.851226 0.5248 0 -0.851226 0.5248 0 -0.856159 0.516713 0 -0.856159 0.516713 0 -0.861015 0.50858 0 -0.861015 0.50858 0 -0.865794 0.500401 0 -0.865794 0.500401 0 -0.870495 0.492177 0 -0.870495 0.492177 0 -0.875118 0.483909 0 -0.875118 0.483909 0 -0.879663 0.475598 0 -0.879663 0.475598 0 -0.884129 0.467244 0 -0.884128 0.467244 0 -0.888515 0.458848 0 -0.888515 0.458848 0 -0.892821 0.450411 0 -0.892821 0.450411 0 -0.897048 0.441933 0 -0.897048 0.441933 0 -0.901194 0.433416 0 -0.901194 0.433416 0 -0.905259 0.42486 0 -0.905259 0.42486 0 -0.909243 0.416266 0 -0.909243 0.416266 0 -0.913145 0.407634 0 -0.913145 0.407634 0 -0.916966 0.398966 0 -0.916966 0.398966 0 -0.920704 0.390262 0 -0.920704 0.390262 0 -0.924359 0.381523 0 -0.924359 0.381523 0 -0.927932 0.37275 0 -0.927932 0.37275 0 -0.931421 0.363943 0 -0.931421 0.363944 0 -0.934827 0.355104 0 -0.934827 0.355104 0 -0.938148 0.346233 0 -0.938149 0.346233 0 -0.941386 0.337331 0 -0.941386 0.337331 0 -0.944539 0.328398 0 -0.944539 0.328398 0 -0.947608 0.319437 0 -0.947608 0.319436 0 -0.950591 0.310446 0 -0.950591 0.310446 0 -0.953489 0.301427 0 -0.953489 0.301427 0 -0.956302 0.292382 0 -0.956302 0.292382 0 -0.959028 0.28331 0 -0.959028 0.28331 0 -0.961669 0.274213 0 -0.961669 0.274213 0 -0.964223 0.265091 0 -0.964223 0.265091 0 -0.966691 0.255945 0 -0.966691 0.255946 0 -0.969072 0.246777 0 -0.969072 0.246777 0 -0.971366 0.237586 0 -0.971366 0.237586 0 -0.973573 0.228374 0 -0.973573 0.228374 0 -0.975693 0.219142 0 -0.975693 0.219142 0 -0.977725 0.20989 0 -0.977725 0.209889 0 -0.979669 0.200618 0 -0.979669 0.200619 0 -0.981526 0.191329 0 -0.981526 0.19133 0 -0.983294 0.182024 0 -0.983294 0.182023 0 -0.984974 0.172701 0 -0.984974 0.172701 0 -0.986566 0.163363 0 -0.986566 0.163363 0 -0.988069 0.154011 0 -0.988069 0.15401 0 -0.989484 0.144644 0 -0.989484 0.144644 0 -0.99081 0.135264 0 -0.99081 0.135265 0 -0.992046 0.125873 0 -0.992046 0.125873 0 -0.993194 0.11647 0 -0.993194 0.11647 0 -0.994253 0.107057 0 -0.994253 0.107057 0 -0.995222 0.0976339 0 -0.995222 0.0976336 0 -0.996103 0.0882019 0 -0.996103 0.0882022 0 -0.996893 0.0787623 0 -0.996893 0.0787626 0 -0.997595 0.0693159 0 -0.997595 0.0693156 0 -0.998207 0.0598628 0 -0.998207 0.059863 0 -0.998729 0.0504045 0 -0.998729 0.0504047 0 -0.999162 0.0409419 0 -0.999162 0.0409418 0 -0.999505 0.0314753 0 -0.999505 0.0314754 0 -0.999758 0.022006 0 -0.999758 0.0220062 0 -0.999921 0.0125349 0 -0.999921 0.0125347 0 -0.999995 0.00306231 0 -0.999995 0.00306252 -0 -0.999979 -0.00641033 -0 -0.999979 -0.00641016 -0 -0.999874 -0.0158823 -0 -0.999874 -0.0158824 -0 -0.999679 -0.025353 -0 -0.999679 -0.0253529 -0 -0.999394 -0.0348215 -0 -0.999394 -0.0348213 -0 -0.999019 -0.0442865 -0 -0.999019 -0.0442868 -0 -0.998555 -0.053748 -0 -0.998555 -0.0537478 -0 -0.998001 -0.0632045 -0 -0.998001 -0.0632043 -0 -0.997357 -0.0726551 -0 -0.997357 -0.0726553 -0 -0.996624 -0.0820995 -0 -0.996624 -0.0820994 -0 -0.995802 -0.0915365 -0 -0.995802 -0.0915363 -0 -0.99489 -0.100965 -0 -0.99489 -0.100965 -0 -0.993889 -0.110385 -0 -0.993889 -0.110385 -0 -0.992799 -0.119795 -0 -0.992799 -0.119794 -0 -0.991619 -0.129193 -0 -0.991619 -0.129193 -0 -0.990351 -0.138581 -0 -0.990351 -0.138581 -0 -0.988994 -0.147956 -0 -0.988994 -0.147956 -0 -0.987548 -0.157317 -0 -0.987548 -0.157318 -0 -0.986014 -0.166665 -0 -0.986014 -0.166665 -0 -0.984391 -0.175998 -0 -0.984391 -0.175997 -0 -0.982679 -0.185314 -0 -0.982679 -0.185314 -0 -0.98088 -0.194615 -0 -0.98088 -0.194614 -0 -0.978992 -0.203897 -0 -0.978992 -0.203897 -0 -0.977017 -0.213162 -0 -0.977017 -0.213162 -0 -0.974954 -0.222407 -0 -0.974954 -0.222407 -0 -0.972803 -0.231632 -0 -0.972803 -0.231632 -0 -0.970566 -0.240837 -0 -0.970566 -0.240837 -0 -0.968241 -0.25002 -0 -0.968241 -0.25002 -0 -0.965829 -0.25918 -0 -0.965829 -0.25918 -0 -0.963331 -0.268317 -0 -0.963331 -0.268318 -0 -0.960746 -0.277431 -0 -0.960746 -0.277431 -0 -0.958075 -0.286519 -0 -0.958075 -0.286519 -0 -0.955318 -0.295581 -0 -0.955317 -0.295582 -0 -0.952475 -0.304618 -0 -0.952475 -0.304617 -0 -0.949547 -0.313626 -0 -0.949547 -0.313626 -0 -0.946533 -0.322607 -0 -0.946533 -0.322607 -0 -0.943435 -0.331558 -0 -0.943435 -0.331558 -0 -0.940252 -0.34048 -0 -0.940252 -0.34048 -0 -0.936984 -0.349371 -0 -0.936984 -0.349372 -0 -0.933633 -0.358232 -0 -0.933633 -0.358231 -0 -0.930197 -0.367059 -0 -0.930198 -0.367059 -0 -0.926679 -0.375854 -0 -0.926679 -0.375854 -0 -0.923077 -0.384615 -0 -0.923077 -0.384615 0 0.00473625 0.999989 0 0.0142086 0.999899 0 0.0142084 0.999899 0 0.0236796 0.99972 0 0.0236794 0.99972 0 0.0331483 0.99945 0 0.0331485 0.99945 0 0.0426143 0.999092 0 0.0426141 0.999092 0 0.0520765 0.998643 0 0.052076 0.998643 0 0.0615335 0.998105 0 0.0615337 0.998105 0 0.0709856 0.997477 0 0.0709854 0.997477 0 0.0804311 0.99676 0 0.0804309 0.99676 0 0.089869 0.995954 0 0.0898692 0.995954 0 0.0992994 0.995058 0 0.0992992 0.995058 0 0.108721 0.994072 0 0.108721 0.994072 0 0.118132 0.992998 0 0.118132 0.992998 0 0.127533 0.991834 0 0.127533 0.991834 0 0.136923 0.990582 0 0.136922 0.990582 0 0.1463 0.98924 0 0.1463 0.98924 0 0.155664 0.98781 0 0.155664 0.98781 0 0.165014 0.986291 0 0.165014 0.986291 0 0.174349 0.984684 0 0.17435 0.984684 0 0.183669 0.982988 0 0.183669 0.982988 0 0.192972 0.981204 0 0.192972 0.981204 0 0.202258 0.979332 0 0.202258 0.979332 0 0.211526 0.977372 0 0.211525 0.977372 0 0.220775 0.975325 0 0.220774 0.975325 0 0.230003 0.97319 0 0.230003 0.97319 0 0.239212 0.970967 0 0.239211 0.970967 0 0.248399 0.968658 0 0.248398 0.968658 0 0.257563 0.966262 0 0.257563 0.966261 0 0.266704 0.963778 0 0.266704 0.963778 0 0.275822 0.961209 0 0.275822 0.961209 0 0.284914 0.958553 0 0.284915 0.958553 0 0.293982 0.955811 0 0.293982 0.955811 0 0.303023 0.952983 0 0.303022 0.952983 0 0.312036 0.95007 0 0.312036 0.95007 0 0.321022 0.947072 0 0.321022 0.947072 0 0.329979 0.943988 0 0.329979 0.943988 0 0.338906 0.94082 0 0.338906 0.94082 0 0.347803 0.937568 0 0.347803 0.937568 0 0.356668 0.934231 0 0.356668 0.934231 0 0.365502 0.930811 0 0.365502 0.930811 0 0.374303 0.927307 0 0.374302 0.927307 0 0.38307 0.923719 0 0.38307 0.92372 0 0.391802 0.920049 0 0.391803 0.920049 0 0.4005 0.916297 0 0.4005 0.916297 0 0.409162 0.912462 0 0.409162 0.912462 0 0.417787 0.908545 0 0.417787 0.908545 0 0.426374 0.904547 0 0.426374 0.904547 0 0.434924 0.900467 0 0.434923 0.900467 0 0.443434 0.896307 0 0.443434 0.896307 0 0.451904 0.892066 0 0.451904 0.892066 0 0.460334 0.887746 0 0.460334 0.887746 0 0.468722 0.883345 0 0.468723 0.883345 0 0.477069 0.878866 0 0.477069 0.878866 0 0.485373 0.874307 0 0.485373 0.874307 0 0.493633 0.86967 0 0.493633 0.86967 0 0.501849 0.864955 0 0.501849 0.864955 0 0.51002 0.860163 0 0.51002 0.860163 0 0.518145 0.855293 0 0.518145 0.855293 0 0.526224 0.850346 0 0.526223 0.850346 0 0.534255 0.845323 0 0.534255 0.845324 0 0.542238 0.840225 0 0.542238 0.840225 0 0.550173 0.835051 0 0.550173 0.835051 0 0.558059 0.829802 0 0.558058 0.829802 0 0.565893 0.824478 0 0.565894 0.824478 0 0.573678 0.819081 0 0.573678 0.819081 0 0.581411 0.81361 0 0.581411 0.81361 0 0.589092 0.808066 0 0.589092 0.808066 0 0.59672 0.802449 0 0.59672 0.80245 0 0.604295 0.796761 0 0.604295 0.796761 0 0.611815 0.791001 0 0.611815 0.791001 0 0.61928 0.78517 0 0.61928 0.78517 0 0.62669 0.779268 0 0.62669 0.779269 0 0.634044 0.773297 0 0.634044 0.773297 0 0.64134 0.767256 0 0.64134 0.767257 0 0.64858 0.761147 0 0.648579 0.761147 0 0.65576 0.754969 0 0.655761 0.754969 0 0.662883 0.748723 0 0.662882 0.748724 0 0.669945 0.742411 0 0.669945 0.742411 0 0.676947 0.736031 0 0.676948 0.736031 0 0.68389 0.729586 0 0.683889 0.729586 0 0.69077 0.723075 0 0.690769 0.723075 0 0.697588 0.716499 0 0.697588 0.716499 0 0.704344 0.709859 0 0.704344 0.709859 0 0.711037 0.703155 0 0.711036 0.703155 0 0.717665 0.696388 0 0.717665 0.696388 0 0.72423 0.689559 0 0.72423 0.689559 0 0.730729 0.682668 0 0.730729 0.682668 0 0.737163 0.675715 0 0.737163 0.675715 0 0.743531 0.668702 0 0.74353 0.668702 0 0.749832 0.661629 0 0.749832 0.661629 0 0.756065 0.654496 0 0.756065 0.654496 0 0.762231 0.647305 0 0.762231 0.647305 0 0.768329 0.640056 0 0.768328 0.640056 0 0.774357 0.632749 0 0.774357 0.632749 0 0.780316 0.625385 0 0.780316 0.625386 0 0.786205 0.617966 0 0.786205 0.617966 0 0.792023 0.610491 0 0.792024 0.61049 0 0.797771 0.602961 0 0.797771 0.602961 0 0.803447 0.595377 0 0.803447 0.595377 0 0.80905 0.587739 0 0.809051 0.587739 0 0.814582 0.580049 0 0.814581 0.580049 0 0.82004 0.572307 0 0.820039 0.572307 0 0.825424 0.564513 0 0.825424 0.564513 0 0.830734 0.556669 0 0.830734 0.556669 0 0.83597 0.548775 0 0.83597 0.548775 0 0.841131 0.540832 0 0.841131 0.540831 0 0.846216 0.532839 0 0.846216 0.53284 0 0.851226 0.5248 0 0.851226 0.5248 0 0.856159 0.516713 0 0.856159 0.516713 0 0.861015 0.50858 0 0.861015 0.50858 0 0.865794 0.500401 0 0.865794 0.500401 0 0.870495 0.492177 0 0.870495 0.492177 0 0.875118 0.483909 0 0.875118 0.48391 0 0.879663 0.475598 0 0.879663 0.475598 0 0.884128 0.467244 0 0.884129 0.467244 0 0.888515 0.458848 0 0.888515 0.458848 0 0.892821 0.450411 0 0.892821 0.450411 0 0.897048 0.441933 0 0.897048 0.441933 0 0.901194 0.433416 0 0.901194 0.433416 0 0.905259 0.42486 0 0.905259 0.42486 0 0.909243 0.416266 0 0.909243 0.416266 0 0.913145 0.407634 0 0.913145 0.407635 0 0.916966 0.398966 0 0.916966 0.398966 0 0.920704 0.390263 0 0.920704 0.390262 0 0.924359 0.381523 0 0.924359 0.381524 0 0.927932 0.37275 0 0.927932 0.37275 0 0.931421 0.363944 0 0.931421 0.363943 0 0.934827 0.355104 0 0.934827 0.355105 0 0.938149 0.346233 0 0.938148 0.346233 0 0.941386 0.337331 0 0.941386 0.337331 0 0.944539 0.328398 0 0.944539 0.328399 0 0.947608 0.319436 0 0.947608 0.319437 0 0.950591 0.310446 0 0.950591 0.310446 0 0.953489 0.301427 0 0.953489 0.301428 0 0.956302 0.292382 0 0.956302 0.292382 0 0.959028 0.28331 0 0.959028 0.28331 0 0.961669 0.274213 0 0.961669 0.274213 0 0.964223 0.265091 0 0.964223 0.265091 0 0.966691 0.255946 0 0.966691 0.255945 0 0.969072 0.246777 0 0.969072 0.246777 0 0.971366 0.237586 0 0.971366 0.237586 0 0.973573 0.228375 0 0.973573 0.228374 0 0.975693 0.219142 0 0.975693 0.219142 0 0.977725 0.209889 0 0.977725 0.20989 0 0.979669 0.200619 0 0.979669 0.200618 0 0.981526 0.19133 0 0.981526 0.19133 0 0.983294 0.182023 0 0.983294 0.182024 0 0.984974 0.172701 0 0.984974 0.172701 0 0.986566 0.163363 0 0.986566 0.163363 0 0.988069 0.15401 0 0.988069 0.154011 0 0.989484 0.144644 0 0.989484 0.144644 0 0.99081 0.135264 0 0.99081 0.135265 0 0.992046 0.125873 0 0.992046 0.125873 0 0.993194 0.11647 0 0.993194 0.11647 0 0.994253 0.107057 0 0.994253 0.107057 0 0.995222 0.0976337 0 0.995222 0.097634 0 0.996103 0.0882023 0 0.996103 0.0882021 0 0.996893 0.0787623 0 0.996893 0.0787626 0 0.997595 0.0693157 0 0.997595 0.069316 0 0.998207 0.0598631 0 0.998207 0.0598629 0 0.998729 0.0504046 0 0.998729 0.0504049 0 0.999162 0.0409417 0 0.999162 0.0409419 0 0.999505 0.0314756 0 0.999505 0.0314752 0 0.999758 0.022006 0 0.999758 0.0220062 0 0.999921 0.0125348 0 0.999921 0.012535 0 0.999995 0.00306249 0 0.999995 0.00306225 0 0.999979 -0.00641042 0 0.999979 -0.00640995 0 0.999874 -0.0158823 0 0.999874 -0.0158821 0 0.999679 -0.0253528 0 0.999679 -0.025353 0 0.999394 -0.0348215 0 0.999394 -0.0348213 0 0.999019 -0.0442867 0 0.999019 -0.0442865 0 0.998555 -0.0537476 0 0.998555 -0.0537481 0 0.998001 -0.0632045 0 0.998001 -0.0632042 0 0.997357 -0.0726553 0 0.997357 -0.0726551 0 0.996624 -0.0820993 0 0.996624 -0.0820995 0 0.995802 -0.0915365 0 0.995802 -0.0915362 0 0.99489 -0.100965 0 0.99489 -0.100965 0 0.993889 -0.110384 0 0.993889 -0.110385 0 0.992799 -0.119795 0 0.992799 -0.119794 0 0.991619 -0.129194 0 0.991619 -0.129193 0 0.990351 -0.138581 0 0.990351 -0.138581 0 0.988994 -0.147956 0 0.988994 -0.147956 0 0.987548 -0.157318 0 0.987548 -0.157317 0 0.986014 -0.166665 0 0.986014 -0.166665 0 0.984391 -0.175998 0 0.984391 -0.175997 0 0.982679 -0.185315 0 0.982679 -0.185314 0 0.98088 -0.194614 0 0.98088 -0.194615 0 0.978992 -0.203897 0 0.978992 -0.203897 0 0.977017 -0.213162 0 0.977017 -0.213162 0 0.974954 -0.222407 0 0.974954 -0.222407 0 0.972803 -0.231632 0 0.972803 -0.231632 0 0.970566 -0.240837 0 0.970566 -0.240837 0 0.968241 -0.250019 0 0.968241 -0.25002 0 0.965829 -0.25918 0 0.965829 -0.25918 0 0.963331 -0.268318 0 0.963331 -0.268317 0 0.960746 -0.277431 0 0.960746 -0.277431 0 0.958075 -0.286519 0 0.958075 -0.286519 0 0.955317 -0.295582 0 0.955318 -0.295581 0 0.952475 -0.304617 0 0.952475 -0.304618 0 0.949547 -0.313626 0 0.949547 -0.313626 0 0.946533 -0.322607 0 0.946533 -0.322607 0 0.943435 -0.331558 0 0.943435 -0.331559 0 0.940252 -0.34048 0 0.940252 -0.34048 0 0.936984 -0.349372 0 0.936984 -0.349371 0 0.933633 -0.358231 0 0.933633 -0.358232 0 0.930197 -0.367059 0 0.930198 -0.367059 0 0.926679 -0.375854 0 0.926679 -0.375854 0 0.923077 -0.384615 0 0.923077 -0.384615 0 -1.83152e-16 1 0 -1.83152e-16 1 0 -1.83152e-16 1 0 -1.83152e-16 1 0 0.941696 0.336466 0 0.938357 0.345669 0 0.941696 0.336466 0 0.944944 0.327231 0 0.948103 0.317964 0 0.948103 0.317964 0 0.934928 0.354839 0 0.938357 0.345669 0 0.934928 0.354839 0 0.954146 0.29934 0 0.957031 0.289985 0 0.957031 0.289985 0 0.95117 0.308667 0 0.95117 0.308667 0 0.954146 0.29934 0 0.965134 0.261756 0 0.962525 0.271192 0 0.965134 0.261756 0 0.962525 0.271192 0 0.959824 0.280602 0 0.959824 0.280602 0 0.972405 0.233301 0 0.972405 0.233301 0 0.970074 0.24281 0 0.96765 0.252295 0 0.970074 0.242809 0 0.96765 0.252295 0 0.976786 0.214218 0 0.978836 0.204645 0 0.978836 0.204645 0 0.974642 0.22377 0 0.974642 0.22377 0 0.976786 0.214217 0 0.984424 0.175812 0 0.982655 0.185441 0 0.984424 0.175812 0 0.982655 0.185441 0 0.980793 0.195052 0 0.980793 0.195052 0 0.989162 0.146828 0 0.989162 0.146828 0 0.987677 0.156504 0 0.986098 0.166166 0 0.987677 0.156504 0 0.986098 0.166166 0 0.993047 0.117716 0 0.993047 0.117717 0 0.994152 0.107989 0 0.991847 0.127433 0 0.990552 0.137137 0 0.991847 0.127433 0 0.996895 0.0787482 0 0.996076 0.0885039 0 0.996076 0.0885042 0 0.995162 0.0982514 0 0.995162 0.0982512 0 0.994152 0.107989 0 0.998245 0.0592153 0 0.998777 0.0494394 0 0.998245 0.059215 0 0.996895 0.0787485 0 0.997618 0.0689852 0 0.997618 0.0689849 0 0.999798 0.0200881 0 0.999554 0.0298753 0 0.999798 0.0200883 0 0.999213 0.0396592 0 0.998777 0.0494398 0 0.999213 0.0396594 0 0.999957 -0.00928034 0 0.999957 -0.00928063 0 1 0.000509585 0 0.999947 0.0102995 0 1 0.000509271 0 0.999947 0.0102992 0 0.999584 -0.0288566 0 0.999253 -0.0386411 0 0.999253 -0.0386413 0 0.999818 -0.0190697 0 0.999818 -0.0190694 0 0.999584 -0.0288568 0 0.996974 -0.0777327 0 0.997687 -0.0679687 0 0.997687 -0.0679684 0 0.998305 -0.0581982 0 0.998827 -0.0484221 0 0.998827 -0.0484218 0 0.995261 -0.0972372 0 0.994262 -0.106976 0 0.995261 -0.0972374 0 0.996974 -0.0777324 0 0.996166 -0.087489 0 0.996165 -0.0874893 0 0.991976 -0.126422 0 0.991977 -0.126422 0 0.990691 -0.136128 0 0.993167 -0.116705 0 0.994262 -0.106976 0 0.993167 -0.116705 0 0.986267 -0.165161 0 0.986267 -0.165162 0 0.987836 -0.155498 0 0.989311 -0.14582 0 0.987836 -0.155498 0 0.989311 -0.14582 0 0.982844 -0.18444 0 0.980991 -0.194053 0 0.980991 -0.194053 0 0.984602 -0.174809 0 0.984602 -0.174809 0 0.982844 -0.18444 0 0.974869 -0.222777 0 0.977004 -0.213222 0 0.974869 -0.222777 0 0.977004 -0.213223 0 0.979044 -0.203648 0 0.979044 -0.203647 0 0.967907 -0.251309 0 0.9654 -0.260773 0 0.967907 -0.251309 0 0.972642 -0.23231 0 0.970321 -0.241821 0 0.970321 -0.241821 0 0.96011 -0.279624 0 0.96011 -0.279624 0 0.957326 -0.28901 0 0.962801 -0.270211 0 0.9654 -0.260772 0 0.962801 -0.270211 0 0.948426 -0.316998 0 0.951484 -0.307698 0 0.951484 -0.307698 0 0.954451 -0.298368 0 0.954451 -0.298368 0 0.957326 -0.28901 0 0.942038 -0.335506 0 0.938708 -0.344713 0 0.938708 -0.344713 0 0.948426 -0.316998 0 0.945277 -0.326268 0 0.945277 -0.326268 0 0.92818 -0.372131 0 0.931779 -0.363026 0 0.928181 -0.37213 0 0.931779 -0.363026 0 0.935288 -0.353886 0 0.935288 -0.353886 0 0.916852 -0.399227 0 0.916852 -0.399227 0 0.920717 -0.390232 0 0.924493 -0.381199 0 0.920717 -0.390232 0 0.924493 -0.3812 0 0.90886 -0.417101 0 0.904733 -0.425979 0 0.904733 -0.425979 0 0.9129 -0.408184 0 0.9129 -0.408184 0 0.90886 -0.417101 0 0.887362 -0.461073 0 0.891834 -0.452364 0 0.891834 -0.452364 0 0.896219 -0.443611 0 0.900519 -0.434816 0 0.900519 -0.434816 0 0.878165 -0.478358 0 0.87344 -0.486932 0 0.878165 -0.478358 0 0.887362 -0.461073 0 0.882806 -0.469738 0 0.882806 -0.469738 0 0.863739 -0.50394 0 0.863739 -0.50394 0 0.858764 -0.512372 0 0.868631 -0.49546 0 0.87344 -0.486932 0 0.868631 -0.49546 0 0.843347 -0.537369 0 0.848567 -0.529087 0 0.848568 -0.529087 0 0.853707 -0.520754 0 0.853706 -0.520755 0 0.858764 -0.512372 0 0.832664 -0.553778 0 0.827203 -0.561903 0 0.832664 -0.553778 0 0.843347 -0.537369 0 0.838046 -0.5456 0 0.838046 -0.5456 0 0.816043 -0.577991 0 0.816043 -0.577991 0 0.810345 -0.585953 0 0.821662 -0.569975 0 0.827203 -0.561903 0 0.821662 -0.569975 0 0.792789 -0.609497 0 0.798718 -0.601706 0 0.798718 -0.601706 0 0.80457 -0.593858 0 0.80457 -0.593858 0 0.810345 -0.585952 0 0.780704 -0.624901 0 0.774548 -0.632515 0 0.780703 -0.624902 0 0.792789 -0.609496 0 0.786784 -0.617228 0 0.786784 -0.617229 0 0.762016 -0.647558 0 0.762016 -0.647558 0 0.75564 -0.654987 0 0.768319 -0.640067 0 0.774549 -0.632514 0 0.768319 -0.640067 0 0.742671 -0.669656 0 0.749192 -0.662353 0 0.742671 -0.669656 0 0.749191 -0.662354 0 0.75564 -0.654987 0 0.722686 -0.691177 0 0.722686 -0.691177 0 0.729418 -0.684069 0 0.73608 -0.676895 0 0.729418 -0.684069 0 0.73608 -0.676895 0 0.709015 -0.705194 0 0.702077 -0.712101 0 0.702077 -0.712101 0 0.715885 -0.698219 0 0.715885 -0.698219 0 0.709015 -0.705194 0 0.680862 -0.732411 0 0.688 -0.72571 0 0.680863 -0.732411 0 0.688 -0.725711 0 0.695072 -0.71894 0 0.695072 -0.71894 0 0.659061 -0.75209 0 0.666392 -0.745601 0 0.666392 -0.745601 0 0.67366 -0.739041 0 0.67366 -0.739042 0 0.64421 -0.764849 0 0.636691 -0.771119 0 0.64421 -0.764849 0 0.659061 -0.752089 0 0.651667 -0.758505 0 0.651666 -0.758506 0 0.629111 -0.777315 0 0.621471 -0.783437 0 0.621471 -0.783437 0 0.629111 -0.777315 0 0.636691 -0.771119 0 0.598197 -0.801349 0 0.606013 -0.795454 0 0.598197 -0.801349 0 0.606013 -0.795455 0 0.613772 -0.789484 0 0.613772 -0.789484 0 0.574406 -0.81857 0 0.582393 -0.812908 0 0.582393 -0.812908 0 0.590323 -0.807167 0 0.590323 -0.807167 0 0.55827 -0.82966 0 0.55012 -0.835085 0 0.558269 -0.82966 0 0.574406 -0.81857 0 0.566365 -0.824154 0 0.566365 -0.824154 0 0.541919 -0.840431 0 0.533665 -0.845696 0 0.533665 -0.845696 0 0.550121 -0.835085 0 0.541919 -0.840431 0 0.517005 -0.855982 0 0.517005 -0.855982 0 0.5086 -0.861003 0 0.52536 -0.85088 0 0.52536 -0.85088 0 0.491645 -0.870796 0 0.500147 -0.865941 0 0.491645 -0.870795 0 0.500147 -0.865941 0 0.5086 -0.861003 0 0.465862 -0.884858 0 0.465861 -0.884858 0 0.474502 -0.880254 0 0.483097 -0.875567 0 0.474502 -0.880255 0 0.483096 -0.875567 0 0.448448 -0.893809 0 0.439676 -0.898157 0 0.448448 -0.893809 0 0.457177 -0.889376 0 0.457176 -0.889376 0 0.430862 -0.902418 0 0.422007 -0.906593 0 0.422007 -0.906593 0 0.439676 -0.898156 0 0.430862 -0.902418 0 0.404176 -0.914681 0 0.404176 -0.914681 0 0.395202 -0.918594 0 0.413111 -0.910681 0 0.413111 -0.910681 0 0.368056 -0.929804 0 0.377141 -0.926156 0 0.377141 -0.926156 0 0.38619 -0.922419 0 0.38619 -0.922419 0 0.395202 -0.918594 0 0.349781 -0.936832 0 0.349781 -0.936832 0 0.358936 -0.933362 0 0.368056 -0.929804 0 0.358935 -0.933362 0 0.331372 -0.9435 0 0.322119 -0.946699 0 0.331372 -0.9435 0 0.340592 -0.940211 0 0.340593 -0.940211 0 0.303522 -0.952825 0 0.303522 -0.952824 0 0.294179 -0.95575 0 0.312835 -0.949807 0 0.322119 -0.946699 0 0.312836 -0.949807 0 0.27541 -0.961327 0 0.284809 -0.958584 0 0.275411 -0.961327 0 0.284808 -0.958584 0 0.294179 -0.95575 0 0.247062 -0.969 0 0.247061 -0.969 0 0.256536 -0.966535 0 0.265986 -0.963977 0 0.256536 -0.966535 0 0.265986 -0.963977 0 0.228042 -0.973651 0 0.218499 -0.975837 0 0.228042 -0.973651 0 0.237563 -0.971372 0 0.237563 -0.971372 0 0.199351 -0.979928 0 0.199352 -0.979928 0 0.189748 -0.981833 0 0.208935 -0.977929 0 0.218499 -0.975837 0 0.208935 -0.977929 0 0.160834 -0.986981 0 0.170489 -0.98536 0 0.170489 -0.98536 0 0.180128 -0.983643 0 0.180127 -0.983643 0 0.189749 -0.981833 0 0.14148 -0.989941 0 0.131781 -0.991279 0 0.141479 -0.989941 0 0.160834 -0.986981 0 0.151164 -0.988509 0 0.151164 -0.988509 0 0.122071 -0.992521 0 0.112348 -0.993669 0 0.112348 -0.993669 0 0.131781 -0.991279 0 0.12207 -0.992521 0 0.0831193 -0.99654 0 0.0928716 -0.995678 0 0.0831195 -0.99654 0 0.0928714 -0.995678 0 0.102615 -0.994721 0 0.102615 -0.994721 0 0.0538194 -0.998551 0 0.0538192 -0.998551 0 0.0635925 -0.997976 0 0.0733595 -0.997306 0 0.0635924 -0.997976 0 0.0733594 -0.997306 0 0.0342587 -0.999413 0 0.0244729 -0.9997 0 0.0244725 -0.9997 0 0.0440409 -0.99903 0 0.0440412 -0.99903 0 0.0342583 -0.999413 -0 -0.00489182 -0.999988 0 0.00489182 -0.999988 -0 -0.00489556 -0.999988 0 0.00489566 -0.999988 0 0.0146845 -0.999892 0 0.014683 -0.999892 -0 -0.0342584 -0.999413 -0 -0.0342586 -0.999413 -0 -0.0244725 -0.9997 -0 -0.0146845 -0.999892 -0 -0.0244729 -0.9997 -0 -0.0146829 -0.999892 -0 -0.0538192 -0.998551 -0 -0.0635923 -0.997976 -0 -0.0635925 -0.997976 -0 -0.0440411 -0.99903 -0 -0.0440409 -0.99903 -0 -0.0538194 -0.998551 -0 -0.0928715 -0.995678 -0 -0.0831194 -0.99654 -0 -0.0928713 -0.995678 -0 -0.0831194 -0.99654 -0 -0.0733595 -0.997306 -0 -0.0733593 -0.997306 -0 -0.12207 -0.992521 -0 -0.122071 -0.992521 -0 -0.112348 -0.993669 -0 -0.102614 -0.994721 -0 -0.112348 -0.993669 -0 -0.102615 -0.994721 -0 -0.151164 -0.988509 -0 -0.151164 -0.988509 -0 -0.160834 -0.986981 -0 -0.131781 -0.991279 -0 -0.131781 -0.991279 -0 -0.14148 -0.989941 -0 -0.189748 -0.981833 -0 -0.180127 -0.983643 -0 -0.180127 -0.983643 -0 -0.170489 -0.98536 -0 -0.170489 -0.98536 -0 -0.160834 -0.986981 -0 -0.208935 -0.977929 -0 -0.218499 -0.975837 -0 -0.208935 -0.977929 -0 -0.189748 -0.981833 -0 -0.199352 -0.979928 -0 -0.199352 -0.979928 -0 -0.237563 -0.971372 -0 -0.237563 -0.971372 -0 -0.247061 -0.969 -0 -0.228042 -0.973651 -0 -0.218499 -0.975837 -0 -0.228042 -0.973651 -0 -0.27541 -0.961327 -0 -0.27541 -0.961327 -0 -0.265986 -0.963977 -0 -0.256536 -0.966535 -0 -0.256536 -0.966535 -0 -0.247061 -0.969 -0 -0.294179 -0.95575 -0 -0.303522 -0.952825 -0 -0.303522 -0.952824 -0 -0.284809 -0.958584 -0 -0.284808 -0.958584 -0 -0.294179 -0.95575 -0 -0.331372 -0.9435 -0 -0.322119 -0.946699 -0 -0.331371 -0.9435 -0 -0.322119 -0.946699 -0 -0.312836 -0.949807 -0 -0.312835 -0.949807 -0 -0.358935 -0.933362 -0 -0.358936 -0.933362 -0 -0.34978 -0.936832 -0 -0.340592 -0.940211 -0 -0.349781 -0.936832 -0 -0.340593 -0.940211 -0 -0.38619 -0.922419 -0 -0.38619 -0.922419 -0 -0.395202 -0.918594 -0 -0.377141 -0.926156 -0 -0.368056 -0.929804 -0 -0.377141 -0.926156 -0 -0.422007 -0.906593 -0 -0.413111 -0.910681 -0 -0.413111 -0.910681 -0 -0.404175 -0.914681 -0 -0.404176 -0.914681 -0 -0.395201 -0.918594 -0 -0.439676 -0.898157 -0 -0.448448 -0.893809 -0 -0.448448 -0.893809 -0 -0.422007 -0.906593 -0 -0.430862 -0.902418 -0 -0.430862 -0.902418 -0 -0.474502 -0.880254 -0 -0.465861 -0.884858 -0 -0.474502 -0.880255 -0 -0.465862 -0.884858 -0 -0.457177 -0.889376 -0 -0.457176 -0.889376 -0 -0.500147 -0.865941 -0 -0.500147 -0.865941 -0 -0.491645 -0.870796 -0 -0.483097 -0.875567 -0 -0.491645 -0.870796 -0 -0.483097 -0.875567 -0 -0.52536 -0.85088 -0 -0.52536 -0.85088 -0 -0.533665 -0.845696 -0 -0.5086 -0.861003 -0 -0.5086 -0.861003 -0 -0.517005 -0.855982 -0 -0.55827 -0.82966 -0 -0.550121 -0.835085 -0 -0.55012 -0.835085 -0 -0.541919 -0.840431 -0 -0.541919 -0.840431 -0 -0.533665 -0.845696 -0 -0.574406 -0.81857 -0 -0.582393 -0.812908 -0 -0.574407 -0.81857 -0 -0.558269 -0.82966 -0 -0.566365 -0.824154 -0 -0.566365 -0.824154 -0 -0.606013 -0.795454 -0 -0.598197 -0.801349 -0 -0.606013 -0.795455 -0 -0.590323 -0.807167 -0 -0.582393 -0.812908 -0 -0.590323 -0.807167 -0 -0.629111 -0.777315 -0 -0.629111 -0.777315 -0 -0.621471 -0.783437 -0 -0.613772 -0.789484 -0 -0.621471 -0.783437 -0 -0.613772 -0.789484 -0 -0.64421 -0.764849 -0 -0.651666 -0.758506 -0 -0.651667 -0.758505 -0 -0.636691 -0.771119 -0 -0.636691 -0.771119 -0 -0.64421 -0.764849 -0 -0.67366 -0.739041 -0 -0.666392 -0.745601 -0 -0.67366 -0.739042 -0 -0.666392 -0.745601 -0 -0.659061 -0.752089 -0 -0.659061 -0.75209 -0 -0.695072 -0.71894 -0 -0.702077 -0.712101 -0 -0.695072 -0.71894 -0 -0.680862 -0.732411 -0 -0.688 -0.725711 -0 -0.688 -0.72571 -0 -0.715885 -0.698219 -0 -0.715885 -0.698219 -0 -0.722686 -0.691177 -0 -0.709015 -0.705194 -0 -0.702077 -0.712101 -0 -0.709015 -0.705194 -0 -0.742671 -0.669656 -0 -0.73608 -0.676895 -0 -0.73608 -0.676895 -0 -0.729418 -0.684069 -0 -0.729418 -0.684069 -0 -0.722686 -0.691177 -0 -0.75564 -0.654987 -0 -0.762016 -0.647558 -0 -0.75564 -0.654987 -0 -0.742671 -0.669656 -0 -0.749191 -0.662354 -0 -0.749192 -0.662353 -0 -0.774549 -0.632514 -0 -0.774548 -0.632515 -0 -0.780704 -0.624901 -0 -0.768319 -0.640067 -0 -0.762016 -0.647558 -0 -0.768319 -0.640067 -0 -0.798718 -0.601706 -0 -0.792789 -0.609496 -0 -0.792789 -0.609497 -0 -0.786784 -0.617229 -0 -0.786784 -0.617229 -0 -0.780703 -0.624902 -0 -0.810345 -0.585953 -0 -0.816043 -0.577991 -0 -0.810345 -0.585953 -0 -0.798718 -0.601706 -0 -0.80457 -0.593858 -0 -0.80457 -0.593858 -0 -0.821662 -0.569975 -0 -0.827203 -0.561903 -0 -0.827203 -0.561903 -0 -0.821662 -0.569975 -0 -0.816043 -0.577991 -0 -0.832664 -0.553778 -0 -0.832664 -0.553778 -0 -0.838046 -0.5456 -0 -0.838046 -0.5456 -0 -0.843347 -0.537369 -0 -0.843347 -0.537369 -0 -0.848567 -0.529087 -0 -0.848568 -0.529087 -0 -0.853707 -0.520754 -0 -0.853706 -0.520755 -0 -0.858764 -0.512372 -0 -0.858764 -0.512372 -0 -0.863738 -0.50394 -0 -0.863739 -0.50394 -0 -0.868631 -0.49546 -0 -0.868631 -0.49546 -0 -0.87344 -0.486933 -0 -0.87344 -0.486932 -0 -0.878165 -0.478358 -0 -0.878165 -0.478358 -0 -0.882806 -0.469738 -0 -0.882806 -0.469738 -0 -0.887362 -0.461073 -0 -0.887362 -0.461073 -0 -0.891834 -0.452364 -0 -0.891834 -0.452364 -0 -0.896219 -0.443611 -0 -0.896219 -0.443611 -0 -0.900519 -0.434816 -0 -0.900519 -0.434816 -0 -0.904733 -0.425979 -0 -0.904733 -0.425979 -0 -0.90886 -0.417101 -0 -0.90886 -0.417101 -0 -0.9129 -0.408184 -0 -0.9129 -0.408184 -0 -0.916852 -0.399227 -0 -0.916852 -0.399227 -0 -0.920717 -0.390232 -0 -0.920717 -0.390232 -0 -0.924493 -0.3812 -0 -0.924493 -0.381199 -0 -0.92818 -0.37213 -0 -0.928181 -0.37213 -0 -0.931779 -0.363026 -0 -0.931779 -0.363026 -0 -0.935288 -0.353887 -0 -0.935288 -0.353886 -0 -0.938708 -0.344713 -0 -0.938708 -0.344713 -0 -0.942038 -0.335506 -0 -0.942038 -0.335507 -0 -0.945277 -0.326268 -0 -0.945277 -0.326268 -0 -0.948426 -0.316998 -0 -0.948426 -0.316998 -0 -0.951484 -0.307698 -0 -0.951484 -0.307698 -0 -0.954451 -0.298368 -0 -0.954451 -0.298368 -0 -0.957326 -0.28901 -0 -0.957326 -0.28901 -0 -0.96011 -0.279624 -0 -0.96011 -0.279624 -0 -0.962801 -0.270211 -0 -0.962801 -0.270211 -0 -0.9654 -0.260773 -0 -0.9654 -0.260773 -0 -0.967907 -0.251309 -0 -0.967907 -0.251309 -0 -0.970321 -0.241821 -0 -0.970321 -0.241821 -0 -0.972642 -0.23231 -0 -0.972642 -0.23231 -0 -0.974869 -0.222777 -0 -0.974869 -0.222777 -0 -0.977004 -0.213223 -0 -0.977004 -0.213222 -0 -0.979044 -0.203647 -0 -0.979044 -0.203647 -0 -0.980991 -0.194053 -0 -0.980991 -0.194053 -0 -0.982844 -0.18444 -0 -0.982844 -0.18444 -0 -0.984602 -0.174809 -0 -0.984602 -0.174809 -0 -0.986267 -0.165161 -0 -0.986266 -0.165162 -0 -0.987836 -0.155498 -0 -0.987836 -0.155498 -0 -0.989311 -0.14582 -0 -0.989311 -0.14582 -0 -0.990691 -0.136127 -0 -0.990691 -0.136128 -0 -0.991976 -0.126422 -0 -0.991977 -0.126422 -0 -0.993167 -0.116705 -0 -0.993167 -0.116705 -0 -0.994262 -0.106976 -0 -0.994262 -0.106976 -0 -0.995261 -0.0972374 -0 -0.995261 -0.0972372 -0 -0.996165 -0.0874894 -0 -0.996166 -0.0874889 -0 -0.996974 -0.0777324 -0 -0.996974 -0.0777328 -0 -0.997687 -0.0679687 -0 -0.997687 -0.0679685 -0 -0.998305 -0.0581983 -0 -0.998305 -0.0581978 -0 -0.998827 -0.0484218 -0 -0.998827 -0.0484222 -0 -0.999253 -0.0386413 -0 -0.999253 -0.0386411 -0 -0.999584 -0.0288568 -0 -0.999584 -0.0288566 -0 -0.999818 -0.0190694 -0 -0.999818 -0.0190696 -0 -0.999957 -0.00928074 -0 -0.999957 -0.00928028 0 -1 0.000509157 0 -1 0.000509613 0 -0.999947 0.0102995 0 -0.999947 0.0102992 0 -0.999798 0.0200882 0 -0.999798 0.0200884 0 -0.999554 0.0298751 0 -0.999554 0.0298753 0 -0.999213 0.0396593 0 -0.999213 0.0396591 0 -0.998777 0.0494394 0 -0.998777 0.0494398 0 -0.998245 0.0592149 0 -0.998245 0.0592154 0 -0.997618 0.0689852 0 -0.997618 0.0689847 0 -0.996895 0.0787481 0 -0.996895 0.0787485 0 -0.996076 0.0885038 0 -0.996076 0.0885043 0 -0.995162 0.0982515 0 -0.995162 0.0982512 0 -0.994152 0.107989 0 -0.994152 0.107989 0 -0.993047 0.117716 0 -0.993047 0.117717 0 -0.991847 0.127433 0 -0.991847 0.127433 0 -0.990552 0.137137 0 -0.990552 0.137137 0 -0.989162 0.146827 0 -0.989162 0.146828 0 -0.987677 0.156504 0 -0.987677 0.156504 0 -0.986098 0.166166 0 -0.986098 0.166166 0 -0.984424 0.175812 0 -0.984424 0.175812 0 -0.982655 0.185441 0 -0.982655 0.185441 0 -0.980793 0.195052 0 -0.980793 0.195052 0 -0.978836 0.204644 0 -0.978836 0.204645 0 -0.976786 0.214218 0 -0.976786 0.214217 0 -0.974642 0.22377 0 -0.974642 0.22377 0 -0.972405 0.233301 0 -0.972405 0.233301 0 -0.970074 0.24281 0 -0.970074 0.242809 0 -0.96765 0.252295 0 -0.96765 0.252295 0 -0.965134 0.261756 0 -0.965134 0.261756 0 -0.962525 0.271192 0 -0.962525 0.271192 0 -0.959824 0.280602 0 -0.959824 0.280602 0 -0.957031 0.289985 0 -0.957031 0.289985 0 -0.954146 0.29934 0 -0.954146 0.29934 0 -0.95117 0.308667 0 -0.95117 0.308667 0 -0.948103 0.317964 0 -0.948103 0.317964 0 -0.944944 0.327231 0 -0.944945 0.327231 0 -0.941696 0.336466 0 -0.941696 0.336466 0 -0.938357 0.345669 0 -0.938357 0.345669 0 -0.934928 0.354839 0 -0.934928 0.354839 -0 -0.680863 -0.732411 -0 -0.598197 -0.801349 -0 -0.517005 -0.855983 -0 -0.439676 -0.898156 -0 -0.368056 -0.929804 -0 -0.265986 -0.963977 -0 -0.141479 -0.989941 0 0.896219 -0.443611 0 0.942038 -0.335507 0 0.972642 -0.23231 0 0.990691 -0.136127 0 0.998305 -0.0581979 0 0.999554 0.0298751 0 0.990552 0.137137 0 0.944945 0.327231 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0

0 1 2 1 3 4 3 1 0 5 4 6 3 6 4 7 8 5 5 6 7 8 9 10 9 8 7 11 10 12 9 12 10 13 14 11 11 12 13 14 15 16 15 14 13 17 16 18 15 18 16 19 20 17 17 18 19 20 21 22 21 20 19 23 22 24 21 24 22 25 26 23 23 24 25 26 27 28 27 26 25 29 28 30 27 30 28 31 32 29 29 30 31 32 33 34 33 32 31 35 34 36 33 36 34 37 38 35 35 36 37 38 39 40 39 38 37 41 40 42 39 42 40 43 44 41 41 42 43 44 45 46 45 44 43 47 46 48 45 48 46 49 50 47 47 48 49 50 51 52 51 50 49 53 52 54 51 54 52 55 56 53 53 54 55 56 57 58 57 56 55 59 58 60 57 60 58 61 62 59 59 60 61 62 63 64 63 62 61 65 64 66 63 66 64 67 68 65 65 66 67 68 69 70 69 68 67 71 70 72 69 72 70 73 74 71 71 72 73 74 75 76 75 74 73 77 76 78 75 78 76 79 80 77 77 78 79 80 81 82 81 80 79 83 82 84 81 84 82 85 86 83 83 84 85 86 87 88 87 86 85 89 88 90 87 90 88 91 92 89 89 90 91 92 93 94 93 92 91 95 94 96 93 96 94 97 98 95 95 96 97 98 99 100 99 98 97 101 100 102 99 102 100 103 104 101 101 102 103 104 105 106 105 104 103 107 106 108 105 108 106 109 110 107 107 108 109 110 111 112 111 110 109 113 112 114 111 114 112 115 116 113 113 114 115 116 117 118 117 116 115 119 118 120 117 120 118 121 122 119 119 120 121 122 123 124 123 122 121 125 124 126 123 126 124 127 128 125 125 126 127 128 129 130 129 128 127 131 130 132 129 132 130 133 134 131 131 132 133 134 135 136 135 134 133 137 136 138 135 138 136 139 140 137 137 138 139 140 141 142 141 140 139 143 142 144 141 144 142 145 146 143 143 144 145 146 147 148 147 146 145 149 148 150 147 150 148 151 152 149 149 150 151 152 153 154 153 152 151 155 154 156 153 156 154 157 158 155 155 156 157 158 159 160 159 158 157 161 160 162 159 162 160 163 164 161 161 162 163 164 165 166 165 164 163 167 166 168 165 168 166 169 170 167 167 168 169 170 171 172 171 170 169 173 172 174 171 174 172 175 176 173 173 174 175 176 177 178 177 176 175 179 178 180 177 180 178 181 182 179 179 180 181 182 183 184 183 182 181 185 184 186 183 186 184 187 188 185 185 186 187 188 189 190 189 188 187 191 190 192 189 192 190 193 194 191 191 192 193 194 195 196 195 194 193 197 196 198 195 198 196 199 200 197 197 198 199 200 201 202 201 200 199 203 202 204 201 204 202 205 206 203 203 204 205 206 207 208 207 206 205 209 208 210 207 210 208 211 212 209 209 210 211 212 213 214 213 212 211 215 214 216 213 216 214 217 218 215 215 216 217 218 219 220 219 218 217 221 220 222 219 222 220 223 224 221 221 222 223 224 225 226 225 224 223 227 226 228 225 228 226 229 230 227 227 228 229 230 231 232 231 230 229 233 232 234 231 234 232 235 236 233 233 234 235 236 237 238 237 236 235 239 238 240 237 240 238 241 242 239 239 240 241 242 243 244 243 242 241 245 244 246 243 246 244 247 248 245 245 246 247 248 249 250 249 248 247 251 250 252 249 252 250 253 254 251 251 252 253 254 255 256 255 254 253 257 256 258 255 258 256 259 260 257 257 258 259 260 261 262 261 260 259 263 262 264 261 264 262 265 266 263 263 264 265 266 267 268 267 266 265 269 268 270 267 270 268 271 272 269 269 270 271 272 273 274 273 272 271 275 274 276 273 276 274 277 278 275 275 276 277 278 279 280 279 278 277 281 280 282 279 282 280 283 284 281 281 282 283 284 285 286 285 284 283 287 286 288 285 288 286 289 290 287 287 288 289 290 291 292 291 290 289 293 292 294 291 294 292 295 296 293 293 294 295 296 297 298 297 296 295 299 298 300 297 300 298 301 302 299 299 300 301 302 303 304 303 302 301 305 304 306 303 306 304 307 308 305 305 306 307 308 309 310 309 308 307 311 310 312 309 312 310 313 314 311 311 312 313 314 315 316 315 314 313 317 316 318 315 318 316 319 320 317 317 318 319 320 321 322 321 320 319 323 322 324 321 324 322 325 326 323 323 324 325 326 327 328 327 326 325 329 328 330 327 330 328 331 332 329 329 330 331 332 333 334 333 332 331 335 334 336 333 336 334 337 338 335 335 336 337 338 339 340 339 338 337 341 340 342 339 342 340 343 344 341 341 342 343 344 345 346 345 344 343 347 346 348 345 348 346 349 350 347 347 348 349 350 351 352 351 350 349 353 352 354 351 354 352 355 356 353 353 354 355 356 357 358 357 356 355 359 358 360 357 360 358 361 362 359 359 360 361 362 363 364 363 362 361 365 364 366 363 366 364 367 368 365 365 366 367 368 369 370 369 368 367 371 370 372 369 372 370 373 374 371 371 372 373 374 375 376 375 374 373 377 376 378 375 378 376 379 380 377 377 378 379 380 381 382 381 380 379 383 382 384 381 384 382 385 386 383 383 384 385 386 387 388 387 386 385 389 388 390 387 390 388 391 392 389 389 390 391 392 393 394 393 392 391 395 394 396 393 396 394 397 398 395 395 396 397 398 399 400 399 398 397 401 400 402 399 402 400 403 404 401 401 402 403 404 405 406 405 404 403 407 406 408 405 408 406 409 410 407 407 408 409 410 411 412 411 410 409 413 412 414 411 414 412 415 416 413 413 414 415 0 2 417 418 419 417 417 2 418 419 420 421 420 419 418 422 421 423 420 423 421 424 425 422 422 423 424 425 426 427 426 425 424 428 427 429 426 429 427 430 431 428 428 429 430 431 432 433 432 431 430 434 433 435 432 435 433 436 437 434 434 435 436 437 438 439 438 437 436 440 439 441 438 441 439 442 443 440 440 441 442 443 444 445 444 443 442 446 445 447 444 447 445 448 449 446 446 447 448 449 450 451 450 449 448 452 451 453 450 453 451 454 455 452 452 453 454 455 456 457 456 455 454 458 457 459 456 459 457 460 461 458 458 459 460 461 462 463 462 461 460 464 463 465 462 465 463 466 467 464 464 465 466 467 468 469 468 467 466 470 469 471 468 471 469 472 473 470 470 471 472 473 474 475 474 473 472 476 475 477 474 477 475 478 479 476 476 477 478 479 480 481 480 479 478 482 481 483 480 483 481 484 485 482 482 483 484 485 486 487 486 485 484 488 487 489 486 489 487 490 491 488 488 489 490 491 492 493 492 491 490 494 493 495 492 495 493 496 497 494 494 495 496 497 498 499 498 497 496 500 499 501 498 501 499 502 503 500 500 501 502 503 504 505 504 503 502 506 505 507 504 507 505 508 509 506 506 507 508 509 510 511 510 509 508 512 511 513 510 513 511 514 515 512 512 513 514 515 516 517 516 515 514 518 517 519 516 519 517 520 521 518 518 519 520 521 522 523 522 521 520 524 523 525 522 525 523 526 527 524 524 525 526 527 528 529 528 527 526 530 529 531 528 531 529 532 533 530 530 531 532 533 534 535 534 533 532 536 535 537 534 537 535 538 539 536 536 537 538 539 540 541 540 539 538 542 541 543 540 543 541 544 545 542 542 543 544 545 546 547 546 545 544 548 547 549 546 549 547 550 551 548 548 549 550 551 552 553 552 551 550 554 553 555 552 555 553 556 557 554 554 555 556 557 558 559 558 557 556 560 559 561 558 561 559 562 563 560 560 561 562 563 564 565 564 563 562 566 565 567 564 567 565 568 569 566 566 567 568 569 570 571 570 569 568 572 571 573 570 573 571 574 575 572 572 573 574 575 576 577 576 575 574 578 577 579 576 579 577 580 581 578 578 579 580 581 582 583 582 581 580 584 583 585 582 585 583 586 587 584 584 585 586 587 588 589 588 587 586 590 589 591 588 591 589 592 593 590 590 591 592 593 594 595 594 593 592 596 595 597 594 597 595 598 599 596 596 597 598 599 600 601 600 599 598 602 601 603 600 603 601 604 605 602 602 603 604 605 606 607 606 605 604 608 607 609 606 609 607 610 611 608 608 609 610 611 612 613 612 611 610 614 613 615 612 615 613 616 617 614 614 615 616 617 618 619 618 617 616 620 619 621 618 621 619 622 623 620 620 621 622 623 624 625 624 623 622 626 625 627 624 627 625 628 629 626 626 627 628 629 630 631 630 629 628 632 631 633 630 633 631 634 635 632 632 633 634 635 636 637 636 635 634 638 637 639 636 639 637 640 641 638 638 639 640 641 642 643 642 641 640 644 643 645 642 645 643 646 647 644 644 645 646 647 648 649 648 647 646 650 649 651 648 651 649 652 653 650 650 651 652 653 654 655 654 653 652 656 655 657 654 657 655 658 659 656 656 657 658 659 660 661 660 659 658 662 661 663 660 663 661 664 665 662 662 663 664 665 666 667 666 665 664 668 667 669 666 669 667 670 671 668 668 669 670 671 672 673 672 671 670 674 673 675 672 675 673 676 677 674 674 675 676 677 678 679 678 677 676 680 679 681 678 681 679 682 683 680 680 681 682 683 684 685 684 683 682 686 685 687 684 687 685 688 689 686 686 687 688 689 690 691 690 689 688 692 691 693 690 693 691 694 695 692 692 693 694 695 696 697 696 695 694 698 697 699 696 699 697 700 701 698 698 699 700 701 702 703 702 701 700 704 703 705 702 705 703 706 707 704 704 705 706 707 708 709 708 707 706 710 709 711 708 711 709 712 713 710 710 711 712 713 714 715 714 713 712 716 715 717 714 717 715 718 719 716 716 717 718 719 720 721 720 719 718 722 721 723 720 723 721 724 725 722 722 723 724 725 726 727 726 725 724 728 727 729 726 729 727 730 731 728 728 729 730 731 732 733 732 731 730 734 733 735 732 735 733 736 737 734 734 735 736 737 738 739 738 737 736 740 739 741 738 741 739 742 743 740 740 741 742 743 744 745 744 743 742 746 745 747 744 747 745 748 749 746 746 747 748 749 750 751 750 749 748 752 751 753 750 753 751 754 755 752 752 753 754 755 756 757 756 755 754 758 757 759 756 759 757 760 761 758 758 759 760 761 762 763 762 761 760 764 763 765 762 765 763 766 767 764 764 765 766 767 768 769 768 767 766 770 769 771 768 771 769 772 773 770 770 771 772 773 774 775 774 773 772 776 775 777 774 777 775 778 779 776 776 777 778 779 780 781 780 779 778 782 781 783 780 783 781 784 785 782 782 783 784 785 786 787 786 785 784 788 787 789 786 789 787 790 791 788 788 789 790 791 792 793 792 791 790 794 793 795 792 795 793 796 797 794 794 795 796 797 798 799 798 797 796 800 799 801 798 801 799 802 803 800 800 801 802 803 804 805 804 803 802 806 805 807 804 807 805 808 809 806 806 807 808 809 810 811 810 809 808 812 811 813 810 813 811 814 815 812 812 813 814 815 816 817 816 815 814 818 817 819 816 819 817 820 821 818 818 819 820 821 822 823 822 821 820 824 823 825 822 825 823 826 827 824 824 825 826 827 828 829 828 827 826 828 830 829 829 830 831 832 833 834 833 832 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1050 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1068 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1083 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1095 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1100 1108 1109 1110 1111 1112 1110 1113 1114 1115 1116 1117 1116 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1120 1129 1130 1131 1132 1133 1127 1134 1135 1136 1137 1138 1136 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1141 1152 1153 1154 1155 1156 1154 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1159 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1176 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1192 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1412 1417 1418 1413 1414 1413 1418 1419 1417 1420 1417 1419 1418 1420 1421 1422 1422 1419 1420 1423 1424 1421 1422 1421 1424 1425 1423 1426 1423 1425 1424 1426 1427 1428 1428 1425 1426 1429 1430 1427 1428 1427 1430 1431 1429 1432 1429 1431 1430 1432 1433 1434 1434 1431 1432 1435 1436 1433 1434 1433 1436 1437 1435 1438 1435 1437 1436 1438 1439 1440 1440 1437 1438 1441 1442 1439 1440 1439 1442 1443 1441 1444 1441 1443 1442 1444 1445 1446 1446 1443 1444 1447 1448 1445 1446 1445 1448 1449 1447 1450 1447 1449 1448 1450 1451 1452 1452 1449 1450 1453 1454 1451 1452 1451 1454 1455 1453 1456 1453 1455 1454 1456 1457 1458 1458 1455 1456 1459 1460 1457 1458 1457 1460 1461 1459 1462 1459 1461 1460 1462 1463 1464 1464 1461 1462 1465 1466 1463 1464 1463 1466 1467 1465 1468 1465 1467 1466 1468 1469 1470 1470 1467 1468 1471 1472 1469 1470 1469 1472 1473 1471 1474 1471 1473 1472 1474 1475 1476 1476 1473 1474 1477 1478 1475 1476 1475 1478 1479 1477 1480 1477 1479 1478 1480 1481 1482 1482 1479 1480 1483 1484 1481 1482 1481 1484 1485 1483 1486 1483 1485 1484 1486 1487 1488 1488 1485 1486 1489 1490 1487 1488 1487 1490 1491 1489 1492 1489 1491 1490 1492 1493 1494 1494 1491 1492 1495 1496 1493 1494 1493 1496 1497 1495 1498 1495 1497 1496 1498 1499 1500 1500 1497 1498 1501 1502 1499 1500 1499 1502 1503 1501 1504 1501 1503 1502 1504 1505 1506 1506 1503 1504 1507 1508 1505 1506 1505 1508 1509 1507 1510 1507 1509 1508 1510 1511 1512 1512 1509 1510 1513 1514 1511 1512 1511 1514 1515 1513 1516 1513 1515 1514 1516 1517 1518 1518 1515 1516 1519 1520 1517 1518 1517 1520 1521 1519 1522 1519 1521 1520 1522 1523 1524 1524 1521 1522 1525 1526 1523 1524 1523 1526 1527 1525 1528 1525 1527 1526 1528 1529 1530 1530 1527 1528 1531 1532 1529 1530 1529 1532 1533 1531 1534 1531 1533 1532 1534 1535 1536 1536 1533 1534 1537 1538 1535 1536 1535 1538 1539 1537 1540 1537 1539 1538 1540 1541 1542 1542 1539 1540 1543 1544 1541 1542 1541 1544 1545 1543 1546 1543 1545 1544 1546 1547 1548 1548 1545 1546 1549 1550 1547 1548 1547 1550 1551 1549 1552 1549 1551 1550 1552 1553 1554 1554 1551 1552 1555 1556 1553 1554 1553 1556 1557 1555 1558 1555 1557 1556 1558 1559 1560 1560 1557 1558 1561 1562 1559 1560 1559 1562 1563 1561 1564 1561 1563 1562 1564 1565 1566 1566 1563 1564 1567 1568 1565 1566 1565 1568 1569 1567 1570 1567 1569 1568 1570 1571 1572 1572 1569 1570 1573 1574 1571 1572 1571 1574 1575 1573 1576 1573 1575 1574 1576 1577 1578 1578 1575 1576 1579 1580 1577 1578 1577 1580 1581 1579 1582 1579 1581 1580 1582 1583 1584 1584 1581 1582 1585 1586 1583 1584 1583 1586 1587 1585 1588 1585 1587 1586 1588 1589 1590 1590 1587 1588 1591 1592 1589 1590 1589 1592 1593 1591 1594 1591 1593 1592 1594 1595 1596 1596 1593 1594 1597 1598 1595 1596 1595 1598 1599 1597 1600 1597 1599 1598 1600 1601 1602 1602 1599 1600 1603 1604 1601 1602 1601 1604 1605 1603 1606 1603 1605 1604 1606 1607 1608 1608 1605 1606 1609 1610 1607 1608 1607 1610 1611 1609 1612 1609 1611 1610 1407 1416 1415 1415 1412 1414 1410 1406 1408 1406 1416 1407 1409 1411 1400 1410 1408 1411 1402 1401 1403 1409 1400 1402 1404 1396 1405 1401 1404 1403 1394 1399 1395 1396 1395 1405 1389 1398 1397 1397 1399 1394 1392 1388 1390 1388 1398 1389 1391 1393 1382 1392 1390 1393 1384 1383 1385 1391 1382 1384 1386 1378 1387 1383 1386 1385 1376 1381 1377 1378 1377 1387 1371 1380 1379 1379 1381 1376 1374 1370 1372 1370 1380 1371 1373 1375 1613 1374 1372 1375 1364 1366 1613 1373 1613 1366 1367 1369 1365 1364 1367 1365 1360 1359 1368 1368 1359 1369 1362 1358 1363 1363 1358 1360 1352 1361 1353 1352 1362 1361 1354 1356 1355 1354 1353 1356 1357 1346 1348 1355 1357 1348 1614 1351 1347 1346 1614 1347 1341 1350 1349 1349 1351 1614 1344 1340 1342 1340 1350 1341 1343 1345 1334 1344 1342 1345 1336 1335 1337 1343 1334 1336 1338 1330 1339 1335 1338 1337 1328 1615 1329 1330 1329 1339 1615 1333 1332 1333 1615 1328 1322 1331 1323 1322 1332 1331 1324 1326 1325 1324 1323 1326 1327 1316 1318 1325 1327 1318 1319 1321 1317 1316 1319 1317 1312 1311 1320 1320 1311 1321 1314 1310 1616 1616 1310 1312 1313 1315 1304 1314 1616 1315 1306 1305 1307 1313 1304 1306 1308 1300 1309 1305 1308 1307 1298 1303 1299 1300 1299 1309 1617 1302 1301 1301 1303 1298 1617 1293 1292 1292 1302 1617 1294 1296 1295 1294 1293 1296 1297 1286 1288 1295 1297 1288 1289 1291 1287 1286 1289 1287 1282 1281 1290 1290 1281 1291 1284 1280 1285 1285 1280 1282 1274 1283 1275 1274 1284 1283 1276 1618 1277 1276 1275 1618 1278 1270 1279 1618 1278 1277 1268 1273 1269 1270 1269 1279 1263 1272 1271 1271 1273 1268 1266 1262 1264 1262 1272 1263 1265 1267 1256 1266 1264 1267 1258 1257 1259 1265 1256 1258 1260 1252 1261 1257 1260 1259 1250 1619 1251 1252 1251 1261 1619 1255 1254 1255 1619 1250 1244 1253 1245 1244 1254 1253 1246 1248 1247 1246 1245 1248 1249 1238 1240 1247 1249 1240 1241 1243 1239 1238 1241 1239 1234 1233 1242 1242 1233 1243 1236 1232 1237 1237 1232 1234 1226 1235 1227 1226 1236 1235 1228 1230 1229 1228 1227 1230 1231 1220 1222 1229 1231 1222 1223 1225 1221 1220 1223 1221 1216 1215 1224 1224 1215 1225 1218 1214 1219 1219 1214 1216 1208 1217 1209 1208 1218 1217 1210 1212 1211 1210 1209 1212 1213 1202 1204 1211 1213 1204 1205 1207 1203 1202 1205 1203 1199 1198 1206 1206 1198 1207 1200 1197 1201 1201 1197 1199 1193 1195 1191 1191 1200 1192 1194 1196 1185 1195 1193 1196 1187 1186 1188 1194 1185 1187 1189 1181 1190 1186 1189 1188 1179 1184 1180 1181 1180 1190 1175 1183 1182 1182 1184 1179 1178 1174 1176 1174 1183 1175 1168 1177 1169 1168 1178 1177 1170 1172 1171 1170 1169 1172 1173 1163 1165 1171 1173 1165 1166 1167 1164 1163 1166 1164 1158 1157 1162 1159 1158 1167 1153 1161 1160 1160 1162 1157 1156 1152 1154 1152 1161 1153 1147 1155 1148 1147 1156 1155 1149 1151 1150 1149 1148 1151 1144 1143 1142 1150 1141 1143 1145 1138 1146 1142 1145 1144 1136 1140 1137 1138 1137 1146 1133 1132 1139 1139 1132 1140 1134 1131 1135 1135 1131 1133 1128 1129 1126 1126 1134 1127 1120 1130 1121 1129 1128 1130 1122 1124 1123 1122 1121 1124 1125 1115 1117 1123 1125 1117 1119 1118 1112 1115 1118 1116 1110 1114 1111 1112 1111 1119 1107 1106 1113 1113 1106 1114 1108 1105 1109 1109 1105 1107 1101 1103 1099 1099 1108 1100 1102 1104 1094 1103 1101 1104 1096 1095 1097 1102 1094 1096 1098 1088 1090 1097 1098 1090 1091 1093 1089 1088 1091 1089 1085 1084 1092 1092 1084 1093 1086 1078 1087 1086 1083 1085 1081 1077 1079 1077 1087 1078 1080 1082 1072 1081 1079 1082 1074 1073 1075 1080 1072 1074 1076 1066 1068 1073 1076 1075 1069 1071 1067 1066 1069 1067 1062 1061 1070 1070 1061 1071 1064 1060 1065 1065 1060 1062 1054 1063 1055 1054 1064 1063 1056 1058 1057 1056 1055 1058 1059 1049 1051 1057 1059 1051 1053 1052 1045 1049 1052 1050 1043 1048 1044 1045 1044 1053 1038 1047 1046 1046 1048 1043 1041 1037 1039 1037 1047 1038 1040 1042 1031 1041 1039 1042 1033 1032 1034 1040 1031 1033 1035 1027 1036 1032 1035 1034 1025 1030 1026 1027 1026 1036 1020 1029 1028 1028 1030 1025 1023 1019 1021 1019 1029 1020 1022 1024 1013 1023 1021 1024 1015 1014 1016 1022 1013 1015 1017 1009 1018 1014 1017 1016 1007 1012 1008 1009 1008 1018 1002 1011 1010 1010 1012 1007 1005 1001 1003 1001 1011 1002 1004 1006 995 1005 1003 1006 997 996 1620 1004 995 997 1000 1620 998 996 998 1620 991 990 999 999 990 1000 993 989 994 994 989 991 983 992 984 983 993 992 985 987 986 985 984 987 988 977 979 986 988 979 980 982 978 977 980 978 973 972 981 981 972 982 975 971 1621 1621 971 973 974 976 965 975 1621 976 967 966 968 974 965 967 969 961 970 966 969 968 959 964 960 961 960 970 954 963 962 962 964 959 957 953 955 953 963 954 956 958 1622 957 955 958 947 949 1622 956 1622 949 950 952 948 947 950 948 943 942 951 951 942 952 945 941 946 946 941 943 935 944 936 935 945 944 937 939 938 937 936 939 940 931 1623 938 940 1623 929 934 930 931 930 1623 924 933 932 932 934 929 927 923 925 923 933 924 926 928 917 927 925 928 919 918 1624 926 917 919 922 1624 920 918 920 1624 913 912 921 921 912 922 915 911 916 916 911 913 905 914 906 905 915 914 907 909 908 907 906 909 910 899 901 908 910 901 1625 904 900 899 1625 900 894 903 902 902 904 1625 897 893 895 893 903 894 896 898 887 897 895 898 889 888 890 896 887 889 891 883 892 888 891 890 881 886 882 883 882 892 1626 885 884 884 886 881 1626 876 875 875 885 1626 877 879 878 877 876 879 880 869 871 878 880 871 872 874 870 869 872 870 865 864 873 873 864 874 867 863 868 868 863 865 857 866 858 857 867 866 859 861 860 859 858 861 862 851 853 860 862 853 854 856 852 851 854 852 847 846 855 855 846 856 849 845 850 850 845 847 840 848 841 840 849 848 839 1627 838 839 841 1627 836 843 837 1627 836 838 842 837 843 1628 1629 1630 1630 1631 1628 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642 1639 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653 1654 1655 1656 1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1669 1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693 1694 1685 1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744 1743 1742 1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768 1769 1770 1771 1772 1773 1774 1775 1776 1777 1768 1778 1779 1780 1781 1782 1783 1784 1785 1779 1782 1786 1780 1783 1787 1788 1785 1789 1774 1787 1776 1790 1773 1791 1792 1793 1794 1795 1794 1792 1796 1767 1797 1798 1793 1799 1770 1800 1772 1801 1802 1763 1798 1803 1804 1762 1805 1765 1800 1806 1807 1766 1804 1803 1756 1808 1809 1757 1810 1806 1760 1811 1812 1758 1813 1808 1754 1752 1814 1815 1816 1811 1748 1746 1817 1818 1819 1749 1814 1820 1821 1751 1822 1817 1744 1823 1824 1825 1826 1825 1821 1827 1828 1824 1737 1745 1829 1830 1831 1736 1832 1739 1828 1833 1834 1741 1831 1835 1731 1836 1730 1837 1838 1733 1834 1839 1840 1735 1836 1725 1841 1842 1843 1726 1839 1727 1844 1728 1845 1846 1842 1720 1847 1848 1849 1850 1845 1850 1851 1848 1852 1853 1854 1722 1713 1855 1714 1856 1716 1857 1852 1858 1859 1717 1707 1860 1856 1708 1861 1862 1712 1863 1858 1864 1710 1704 1703 1865 1861 1701 1866 1867 1868 1864 1704 1869 1870 1705 1871 1866 1697 1872 1873 1695 1700 1874 1869 1875 1876 1698 1877 1872 1691 1878 1879 1692 1880 1875 1694 1881 1882 1883 1878 1882 1884 1885 1883 1886 1887 1686 1688 1689 1888 1889 1886 1680 1885 1890 1891 1678 1681 1892 1888 1893 1894 1682 1890 1674 1895 1896 1675 1897 1677 1898 1893 1899 1900 1901 1900 1902 1897 1901 1903 1904 1905 1671 1667 1670 1906 1907 1908 1903 1663 1909 1910 1661 1907 1911 1664 1912 1913 1666 1909 1656 1914 1915 1657 1916 1660 1913 1917 1918 1659 1919 1651 1920 1916 1649 1921 1922 1918 1923 1653 1652 1924 1925 1926 1921 1644 1927 1645 1928 1648 1925 1646 1647 1929 1930 1928 1638 1931 1932 1933 1633 1933 1934 1929 1935 1936 1937 1935 1938 1641 1636 1939 1936 1940 1941 1634 1942 1943 1944 1945 1940 1942 1946 1947 1943 1948 1949 1637 1950 1946 1951 1952 1948 1953 1954 1955 1951 1956 1957 1953 1954 1958 1959 1960 1957 1956 1961 1962 1959 1960 1963 1964 1965 1966 1962 1967 1964 1968 1969 1970 1966 1967 1971 1972 1973 1970 1974 1975 1972 1976 1977 1973 1978 1979 1980 1975 1981 1977 1982 1980 1983 1984 1985 1981 1986 1987 1984 1988 1985 1989 1990 1991 1992 1987 1990 1993 1994 1995 1992 1996 1994 1997 1998 1999 2000 1995 1998 2001 2002 2000 2003 2004 2005 2006 2002 2007 2004 2008 2009 2010 2006 2011 2012 2007 2013 2010 2014 2012 2015 2016 2017 2018 2016 2016 2015 2017 2018 2019 2020 2019 2018 2017 2021 2022 2023 2019 2024 2020 2025 2020 2024 2025 2024 2026 2026 2027 2025 2028 2027 2026 2027 2028 2029 2028 2030 2029 2031 2029 2030 2031 2030 2032 2032 2033 2031 2034 2033 2032 2035 2036 2034 2033 2034 2036 2035 2037 2038 2038 2036 2035 2039 2040 2041 2037 2042 2038 2042 2037 2043 2042 2043 2044 2045 2044 2043 2046 2044 2045 2045 2047 2046 2047 2048 2046 2048 2047 2049 2049 2050 2048 2051 2048 2050 2051 2050 2052 2052 2053 2051 2054 2053 2052 2053 2054 2055 2054 2056 2055 2057 2055 2056 2057 2056 2058 2059 2058 2060 2058 2059 2057 2061 2062 2060 2059 2060 2062 2063 2064 2065 2066 2062 2061 2061 2067 2066 2067 2068 2066 2068 2067 2069 2068 2069 2070 2071 2070 2069 2072 2070 2071 2071 2073 2072 2073 2074 2072 2074 2073 2075 2074 2075 2076 2077 2076 2075 2078 2076 2077 2077 2079 2078 2079 2080 2078 2080 2079 2081 2080 2081 2082 2083 2082 2081 2084 2082 2083 2083 2085 2084 2085 2086 2084 2086 2087 2088 2088 2089 2090 2086 2085 2087 2091 2090 2092 2089 2088 2087 2093 2094 2091 2089 2092 2090 2094 2095 2096 2091 2092 2093 2097 2096 2098 2095 2094 2093 2098 2099 2097 2095 2098 2096 2100 2099 2101 2099 2100 2097 2102 2103 2101 2100 2101 2103 2102 2104 2105 2105 2103 2102 2106 2104 2107 2104 2106 2105 2108 2109 2107 2106 2107 2109 2108 2110 2111 2111 2109 2108 2112 2110 2113 2110 2112 2111 2114 2115 2113 2112 2113 2115 2114 2116 2117 2117 2115 2114 2118 2116 2119 2116 2118 2117 2120 2121 2119 2118 2119 2121 2120 2122 2123 2123 2121 2120 2124 2122 2125 2122 2124 2123 2126 2127 2125 2124 2125 2127 2126 2128 2129 2129 2127 2126 2130 2128 2131 2128 2130 2129 2132 2133 2131 2130 2131 2133 2132 2134 2135 2135 2133 2132 2136 2137 2138 2136 2139 2140 2137 2134 2138 2141 2140 2142 2139 2136 2138 2143 2144 2141 2139 2142 2140 2144 2145 2146 2141 2142 2143 2147 2146 2148 2145 2144 2143 2149 2150 2147 2145 2148 2146 2150 2151 2152 2147 2148 2149 2153 2152 2154 2151 2150 2149 2155 2156 2153 2151 2154 2152 2156 2157 2158 2153 2154 2155 2159 2158 2160 2157 2156 2155 2161 2162 2159 2157 2160 2158 2162 2163 2164 2159 2160 2161 2165 2164 2166 2163 2162 2161 2167 2165 2168 2163 2166 2164 2169 2170 2167 2166 2168 2165 2170 2171 2172 2173 2174 2175 2176 2172 2177 2178 2179 2180 2177 2181 2176 2182 2183 2184 2185 2176 2181 2186 2187 2188 2181 2189 2185 2190 2191 2192 2193 2185 2189 2194 2195 2196 2189 2197 2193 2198 2199 2200 2201 2193 2197 2202 2203 2204 2197 2205 2201 2206 2207 2208 2209 2201 2205 2210 2211 2212 2205 2213 2209 2214 2215 2216 2217 2209 2213 2218 2219 2220 2213 2221 2217 2222 2223 2224 2225 2217 2221 2226 2227 2228 2221 2229 2225 2230 2231 2232 2233 2225 2229 2234 2235 2236 2229 2237 2233 2238 2239 2240 2241 2233 2237 2242 2243 2244 2237 2245 2241 2246 2247 2248 2249 2241 2245 2250 2251 2252 2245 2253 2249 2254 2255 2256 2257 2249 2253 2258 2259 2254 2253 2260 2257 2261 2262 2263 2264 2257 2260 2265 2266 2267 2260 2268 2264 2269 2270 2271 2272 2264 2268 2273 2274 2275 2268 2276 2272 2277 2278 2279 2280 2272 2276 2281 2282 2283 2276 2284 2280 2285 2286 2287 2288 2284 2289 2280 2284 2288 2290 2289 2291 2292 2293 2294 2290 2291 2295 2296 2294 2293 2297 2290 2295 2293 2298 2296 2297 2299 2290 2298 2293 2300 2301 2290 2299 2302 2300 2293 2290 2302 2293 2290 2301 2302 2303 2304 2305 2306 2307 2308 2171 2177 2172 2289 2290 2288 2171 2170 2169 2169 2167 2168 2173 2175 2178 2309 2174 2310 2311 2310 2312 2312 2313 2314 2313 2315 2316 2315 2317 2318 2317 2319 2320 2319 2321 2322 2321 2323 2324 2323 2325 2326 2325 2327 2328 2327 2329 2330 2331 2332 2333 2333 2334 2335 2335 2336 2337 2337 2338 2339 2339 2340 2341 2342 2343 2341 2134 2137 2135 2344 2345 2343 2346 2345 2347 2348 2346 2349 2350 2348 2351 2350 2352 2353 2353 2354 2355 2355 2356 2357 2357 2358 2359 2359 2360 2361 2361 2362 2363 2363 2364 2365 2366 2367 2365 2368 2369 2367 2370 2371 2369 2309 2175 2174 2372 2373 2371 2311 2309 2310 2374 2373 2375 2314 2311 2312 2376 2374 2377 2316 2314 2313 2378 2379 2376 2318 2316 2315 2380 2379 2381 2320 2318 2317 2382 2381 2383 2322 2320 2319 2383 2384 2382 2324 2322 2321 2384 2385 2386 2326 2324 2323 2387 2388 2386 2328 2326 2325 2389 2390 2388 2330 2328 2327 2391 2392 2390 2331 2330 2329 2393 2394 2392 2394 2395 2396 2396 2397 2398 2398 2399 2400 2400 2401 2402 2403 2404 2402 2405 2406 2404 2407 2408 2406 2065 2408 2409 2410 2064 2411 2410 2412 2413 2414 2413 2415 2416 2414 2417 2416 2418 2419 2419 2420 2421 2422 2421 2423 2424 2422 2425 2426 2424 2427 2426 2428 2041 2040 2429 2430 2431 2432 2430 2433 2434 2432 2435 2436 2434 2437 2436 2438 2437 2438 2439 2022 2440 2439 2441 2023 2442 2442 2013 2443 2015 2012 2011 2011 2007 2008 2008 2004 2003 2003 2000 1999 1999 1995 1996 1996 1992 1991 1991 1987 1988 1988 1984 1983 1983 1980 1979 1979 1975 1976 1976 1972 1971 1971 1967 1968 1968 1964 1963 1963 1960 1956 1957 1952 1953 1952 1949 1948 1949 1635 1637 1635 1939 1636 1939 1937 1936 1937 1938 1935 1938 1642 1641 1642 1640 1639 1640 1931 1638 1931 1927 1928 1927 1643 1645 1643 1926 1644 1926 1922 1921 1922 1650 1649 1650 1920 1651 1920 1915 1916 1915 1655 1657 1655 1914 1656 1914 1910 1909 1910 1662 1661 1662 1908 1663 1908 1904 1903 1904 1899 1901 1899 1902 1900 1902 1896 1897 1896 1673 1675 1673 1895 1674 1895 1891 1890 1891 1679 1678 1679 1885 1680 1885 1881 1883 1881 1884 1882 1884 1879 1878 1879 1690 1692 1690 1877 1691 1877 1873 1872 1873 1696 1695 1696 1871 1697 1871 1867 1866 1867 1702 1701 1702 1865 1703 1865 1862 1861 1862 1709 1708 1709 1860 1707 1860 1855 1856 1855 1715 1714 1715 1854 1713 1854 1723 1722 1723 1719 1721 1719 1847 1720 1847 1843 1842 1843 1724 1726 1724 1841 1725 1841 1730 1836 1837 1730 1732 1732 1731 1835 1835 1831 1830 1830 1736 1738 1738 1737 1829 1829 1745 1742 1743 1822 1744 1822 1818 1817 1818 1747 1746 1747 1816 1748 1816 1812 1811 1812 1759 1758 1759 1810 1760 1810 1807 1806 1807 1764 1766 1764 1800 1765 1805 1800 1801 1801 1772 1771 1771 1770 1799 1799 1793 1795 1795 1794 1796 1796 1792 1791 1791 1773 1775 1775 1774 1789 1789 1785 1784 1784 1779 1778 1778 1780 1786 1786 1782 1781 1781 1783 1788 1788 1787 1790 1776 1787 1777 1768 1777 1769 1767 1769 1797 1798 1797 1802 1763 1802 1761 1762 1761 1803 1804 1756 1755 1755 1757 1809 1809 1808 1813 1813 1754 1753 1753 1752 1815 1814 1752 1819 1749 1819 1750 1751 1750 1820 1821 1820 1826 1825 1826 1823 1824 1823 1827 1828 1827 1832 1739 1832 1740 1741 1740 1833 1834 1833 1838 1733 1838 1734 1735 1734 1839 1844 1840 1839 1729 1844 1727 1846 1729 1728 1851 1846 1845 1849 1851 1850 1853 1849 1848 1857 1853 1852 1718 1857 1716 1859 1718 1717 1863 1859 1858 1711 1863 1712 1864 1711 1710 1706 1868 1704 1870 1706 1705 1874 1870 1869 1699 1874 1700 1876 1699 1698 1880 1876 1875 1693 1880 1694 1684 1693 1685 1887 1684 1686 1687 1887 1688 1889 1687 1689 1892 1889 1888 1683 1892 1681 1894 1683 1682 1898 1894 1893 1676 1898 1677 1668 1676 1669 1905 1667 1669 1672 1671 1905 1906 1670 1672 1911 1907 1906 1665 1664 1911 1912 1666 1665 1917 1913 1912 1658 1660 1917 1919 1659 1658 1923 1918 1919 1654 1653 1923 1924 1652 1654 1646 1925 1924 1930 1648 1647 1934 1930 1929 1932 1934 1933 1632 1932 1633 1941 1632 1634 1945 1941 1940 1944 1945 1942 1947 1944 1943 1950 1947 1946 1955 1950 1951 1958 1954 1951 1961 1959 1958 1965 1962 1961 1969 1966 1965 1974 1970 1969 1978 1973 1974 1982 1977 1978 1986 1981 1982 1989 1985 1986 1993 1990 1989 1997 1994 1993 2001 1998 1997 2005 2002 2001 2009 2006 2005 2014 2010 2009 2443 2013 2014 2441 2442 2443 2021 2023 2441 2440 2022 2021 2437 2439 2440 2435 2438 2436 2433 2435 2434 2431 2433 2432 2429 2431 2430 2039 2429 2040 2428 2039 2041 2427 2428 2426 2425 2427 2424 2423 2425 2422 2420 2423 2421 2418 2420 2419 2417 2418 2416 2415 2417 2414 2412 2415 2413 2411 2412 2410 2063 2411 2064 2409 2063 2065 2407 2409 2408 2405 2407 2406 2403 2405 2404 2401 2403 2402 2399 2401 2400 2397 2399 2398 2395 2397 2396 2393 2395 2394 2391 2393 2392 2389 2391 2390 2387 2389 2388 2385 2387 2386 2383 2385 2384 2381 2382 2380 2379 2380 2376 2377 2378 2376 2375 2377 2374 2372 2375 2373 2370 2372 2371 2368 2370 2369 2366 2368 2367 2364 2366 2365 2362 2364 2363 2360 2362 2361 2358 2360 2359 2356 2358 2357 2354 2356 2355 2352 2354 2353 2351 2352 2350 2349 2351 2348 2347 2349 2346 2344 2347 2345 2342 2344 2343 2340 2342 2341 2338 2340 2339 2336 2338 2337 2334 2336 2335 2332 2334 2333 2329 2332 2331 2183 2180 2179 2180 2173 2178 2186 2182 2184 2184 2183 2179 2187 2191 2188 2186 2188 2182 2192 2194 2190 2187 2192 2191 2196 2195 2199 2190 2194 2196 2198 2200 2202 2199 2195 2200 2207 2204 2203 2204 2198 2202 2210 2206 2208 2208 2207 2203 2211 2215 2212 2210 2212 2206 2216 2218 2214 2211 2216 2215 2220 2219 2223 2214 2218 2220 2222 2224 2226 2223 2219 2224 2231 2228 2227 2228 2222 2226 2234 2230 2232 2232 2231 2227 2235 2239 2236 2234 2236 2230 2240 2242 2238 2235 2240 2239 2244 2243 2247 2238 2242 2244 2246 2248 2250 2247 2243 2248 2255 2252 2251 2252 2246 2250 2258 2254 2256 2256 2255 2251 2262 2259 2258 2263 2267 2261 2262 2261 2259 2265 2270 2266 2263 2265 2267 2269 2271 2275 2266 2270 2269 2274 2273 2278 2275 2271 2273 2283 2277 2279 2277 2274 2278 2286 2282 2281 2281 2283 2279 2287 2306 2285 2286 2285 2282 2307 2303 2308 2287 2307 2306 2305 2304 2292 2308 2303 2305 2293 2292 2304 2444 2445 2446 2447 2448 2449 2450 2451 2452 2453 2454 2455 2456 2457 2458 2459 2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470 2471 2472 2473 2474 2475 2476 2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488 2489 2490 2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2517 2516 2519 2520 2521 2522 2523 2522 2521 2524 2525 2526 2526 2527 2524 2528 2529 2530 2529 2528 2531 2532 2533 2534 2535 2534 2533 2536 2537 2538 2491 2490 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2569 2578 2579 2580 2575 2574 2581 2582 2583 2584 2585 2578 2580 2586 2587 2588 2582 2589 2583 2590 2591 2592 2588 2587 2593 2594 2595 2596 2597 2590 2592 2598 2599 2600 2601 2602 2603 2604 2605 2606 2600 2599 2607 2608 2609 2610 2611 2604 2606 2612 2613 2614 2608 2615 2609 2616 2617 2618 2619 2612 2620 2621 2622 2623 2624 2616 2618 2625 2626 2627 2628 2629 2630 2631 2632 2633 2634 2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671 2672 2673 2674 2675 2676 2677 2678 2679 2680 2681 2682 2683 2684 2685 2686 2687 2688 2689 2690 2691 2692 2693 2694 2695 2696 2697 2698 2699 2700 2701 2702 2703 2704 2705 2706 2707 2708 2709 2710 2711 2712 2713 2714 2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737 2738 2739 2740 2741 2742 2743 2744 2745 2746 2747 2748 2749 2750 2751 2752 2753 2754 2755 2756 2757 2758 2759 2760 2761 2762 2763 2764 2765 2766 2767 2768 2769 2770 2771 2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806 2807 2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819 2820 2821 2822 2816 2823 2822 2818 2824 2814 2821 2825 2826 2819 2827 2828 2829 2808 2826 2830 2827 2811 2831 2810 2832 2833 2834 2812 2835 2832 2802 2803 2836 2837 2838 2833 2805 2806 2839 2840 2836 2797 2841 2842 2796 2798 2839 2843 2801 2844 2800 2845 2791 2842 2846 2847 2790 2848 2849 2795 2844 2850 2794 2851 2784 2847 2852 2786 2853 2854 2855 2788 2850 2856 2787 2857 2853 2779 2858 2780 2859 2860 2781 2783 2856 2861 2862 2782 2859 2773 2863 2774 2864 2865 2861 2777 2866 2867 2868 2775 2864 2767 2869 2768 2870 2871 2867 2769 2872 2873 2874 2770 2870 2761 2875 2876 2877 2760 2874 2878 2765 2879 2880 2764 2877 2881 2754 2882 2883 2756 2880 2884 2757 2885 2886 2759 2883 2887 2748 2750 2888 2889 2751 2886 2890 2753 2891 2892 2889 2893 2743 2742 2894 2895 2746 2892 2896 2745 2897 2898 2739 2895 2899 2741 2740 2900 2901 2447 2898 2902 2449 2903 2741 2904 2733 2735 2905 2906 2907 2737 2902 2908 2736 2909 2727 2906 2910 2911 2729 2912 2908 2913 2731 2914 2915 2730 2916 2722 2911 2917 2918 2721 2726 2915 2919 2724 2920 2725 2921 2717 2918 2716 2922 2923 2924 2719 2920 2925 2926 2718 2923 2927 2709 2711 2928 2929 2926 2930 2714 2713 2931 2932 2703 2929 2933 2934 2705 2935 2706 2932 2936 2937 2708 2938 2699 2939 2935 2940 2941 2697 2942 2701 2937 2943 2944 2700 2940 2691 2945 2946 2692 2947 2944 2948 2696 2695 2949 2950 2951 2947 2686 2952 2953 2687 2954 2688 2950 2955 2690 2956 2681 2957 2952 2958 2679 2959 2955 2960 2684 2961 2962 2683 2958 2959 2675 2674 2963 2964 2676 2962 2965 2966 2678 2967 2668 2964 2968 2969 2667 2970 2971 2671 2966 2972 2973 2670 2974 2662 2969 2661 2975 2976 2666 2973 2977 2665 2978 2979 2657 2976 2980 2981 2656 2982 2979 2978 2660 2658 2983 2984 2650 2985 2982 2986 2651 2987 2988 2983 2652 2989 2653 2990 2644 2991 2987 2992 2645 2993 2990 2648 2994 2646 2995 2996 2997 2993 2638 2998 2639 2999 3000 2995 2640 3001 2999 2631 3002 3003 2636 3004 2641 3003 2626 3005 3006 3007 2632 3006 3008 3009 2629 2634 3009 3008 2630 3010 2628 3009 2630 2629 3011 2624 3012 3010 3012 2628 3011 2616 2624 3010 3011 3012 2620 3013 3014 3013 2618 2617 2619 2620 3014 3014 3013 2617 2613 2615 2614 2619 2613 2612 3015 3016 3017 2615 2608 2614 2611 3018 3019 3019 2610 2609 2611 2606 3018 3019 3018 2610 2607 3020 3021 3020 2605 2604 2600 2607 3021 3021 3020 2604 3022 2602 2601 2599 2598 3022 2603 2596 2601 3022 2598 2602 2597 2595 2594 2594 2596 2603 2597 2592 2595 3023 3024 3025 3026 3027 2593 2591 2590 3027 2587 3026 2593 3026 2591 3027 3028 2589 3029 2588 3028 2586 3029 2589 2582 2586 3028 3029 2585 3030 3031 3031 2584 2583 2585 2580 3030 3031 3030 2584 3032 3033 2581 2579 2578 3033 2574 3032 2581 3032 2579 3033 3034 2577 3035 2575 3034 2573 3035 2577 2576 2573 3034 3035 2572 2568 2567 2568 2576 2569 2571 2570 2566 2572 2567 2570 2565 2564 2559 2571 2566 2565 2560 2563 2558 2564 2560 2559 2561 2552 2562 2563 2562 2558 2556 2554 2553 2553 2552 2561 2548 2555 2557 2555 2554 2556 2547 2546 2549 2548 2557 2546 2551 2550 2541 2547 2549 2551 2542 2545 2540 2550 2542 2541 2543 2536 2544 2545 2544 2540 3036 2538 2537 2537 2536 2543 2532 3037 3038 3039 2538 3036 2532 2534 3037 3038 3037 3039 3040 3041 2530 2535 2533 3041 2529 3040 2530 3040 2535 3041 3042 2525 3043 2528 3042 2531 3043 2525 2524 2531 3042 3043 2520 3044 3045 3045 2527 2526 2520 2522 3044 3045 3044 2527 3046 3047 2518 2523 2521 3047 2517 3046 2518 3046 2523 3047 3048 2511 3049 2516 3048 2519 2512 3050 2510 2511 2510 3049 2504 2506 3051 2453 3052 2454 3053 2508 3054 3055 2513 3053 2498 2500 2505 2504 2498 2505 2501 3056 3057 2509 3057 3058 2499 2493 2492 2500 2499 2492 3059 3060 2496 3061 3062 2502 2494 2487 2486 2494 2493 2487 3063 2489 3064 2495 3065 3064 2481 2488 2482 2481 2486 2488 3066 3067 2484 3066 2491 2539 2475 2480 2476 2476 2480 2482 3068 2477 3069 2483 3070 3069 2468 2470 2474 2474 2470 2475 2473 3071 3072 3073 3071 2479 2462 2464 2469 2468 2462 2469 3074 3075 2466 3074 2472 3076 2463 2457 2456 2464 2463 2456 2459 3077 3078 2465 2467 3078 2458 2451 2450 2458 2457 2451 3079 3080 2455 3080 3081 2460 3052 2453 3082 3083 2450 2452 3084 3085 3086 3087 3082 3085 3088 3089 3086 2444 2446 3090 3091 2446 3092 3091 3090 2446 2446 3093 3092 3083 2452 2446 2446 2445 3083 3050 2512 3051 2506 3050 3051 2455 2454 3079 3094 2515 3095 3080 3079 3081 3049 2519 3048 3081 2461 2460 3096 3097 3094 2461 3077 2459 3098 3099 3097 3099 3100 3101 3078 3077 2465 3101 3102 3103 2466 3075 2467 3103 3104 3105 3074 3076 3075 3105 3106 3107 2472 2471 3076 3107 3108 3109 2473 3072 2471 3109 3110 3111 3071 3073 3072 3111 3112 3113 2479 2478 3073 3113 3114 3115 2477 3068 2478 3115 3116 3117 3069 3070 3068 3118 3119 3120 2483 2485 3070 3119 3121 3122 2484 3067 2485 3121 3123 3124 3066 2539 3067 3123 3125 3126 3125 3127 3128 3129 3127 3130 3039 3036 3038 2490 2489 3063 3131 3130 3132 3063 3064 3065 3133 3134 3132 3065 2495 2497 3135 3136 3133 2497 2496 3060 3137 3138 3135 3137 3139 3140 3062 3061 3059 3139 3141 3142 2503 2502 3062 3141 3143 3144 3056 2501 2503 3143 3145 3146 3058 3057 3056 3145 3147 3148 2507 2509 3058 3147 3149 3150 3054 2508 2507 3149 3151 3152 3055 3053 3054 3153 3151 3154 2514 2513 3055 3155 3154 3156 3095 2515 2514 3157 3156 3158 3096 3094 3095 3159 3158 3160 3098 3097 3096 3161 3162 3160 3100 3099 3098 3163 3164 3161 3102 3101 3100 3165 3163 3166 3104 3103 3102 3167 3168 3166 3106 3105 3104 3169 3170 3168 3108 3107 3106 3170 3169 3171 3110 3109 3108 3171 3172 3173 3112 3111 3110 3174 3172 3175 3114 3113 3112 3176 3175 3177 3116 3115 3114 3178 3177 3179 3118 3117 3116 3180 3179 3181 3181 3182 3183 3182 3184 3185 3184 3186 3187 3186 3188 3189 3190 3188 3191 3192 3191 3193 3194 3193 3195 3025 3196 3195 3197 3198 3023 3197 3199 3200 3201 3202 3199 3203 3204 3201 3203 3205 3206 3205 3207 3208 3209 3210 3207 3211 3212 3209 3213 3214 3211 3213 3016 3215 3017 3216 3217 3218 3216 3219 3220 3219 3221 3222 3221 3223 3224 3225 3223 3224 3226 3225 2621 3226 3227 3228 3229 2623 3229 3230 2627 3008 2635 2634 2635 3002 2636 3002 3004 3003 3004 2642 2641 2642 3000 2640 3000 2996 2995 2996 2647 2646 2647 2994 2648 2994 2989 2990 2989 2654 2653 2654 2988 2652 2988 2984 2983 2984 2659 2658 2659 2979 2660 2978 2665 2664 2664 2666 2977 2977 2973 2972 2972 2670 2672 2672 2671 2971 2971 2966 2967 2967 2678 2677 2677 2676 2965 2965 2962 2961 2961 2683 2682 2682 2684 2960 2960 2955 2956 2956 2690 2689 2689 2688 2954 2954 2950 2949 2949 2695 2694 2694 2696 2948 2948 2944 2943 2943 2700 2702 2702 2701 2942 2942 2937 2938 2938 2708 2707 2707 2706 2936 2936 2932 2931 2931 2713 2712 2712 2714 2930 2930 2926 2925 2925 2718 2720 2720 2719 2924 2924 2920 2724 2724 2726 2919 2919 2915 2914 2914 2730 2732 2732 2731 2913 2913 2908 2909 2909 2736 2738 2738 2737 2907 2907 2902 2903 2903 2449 2448 2448 2447 2901 2901 2898 2897 2897 2745 2747 2747 2746 2896 2896 2892 2891 2891 2753 2752 2752 2751 2890 2890 2886 2885 2885 2759 2758 2758 2757 2884 2884 2880 2879 2879 2764 2763 2763 2765 2878 2878 2874 2873 2873 2770 2769 2771 2872 2769 2872 2868 2867 2868 2776 2775 2776 2866 2777 2866 2862 2861 2862 2783 2782 2781 2856 2857 2857 2787 2789 2789 2788 2855 2855 2850 2851 2851 2794 2793 2793 2795 2849 2849 2844 2845 2845 2800 2799 2799 2801 2843 2843 2839 2806 2840 2807 2806 2807 2838 2805 2838 2834 2833 2834 2813 2812 2813 2830 2811 2830 2828 2827 2828 2817 2819 2817 2824 2818 2824 2815 2814 2815 2823 2816 2823 2820 2822 2820 2825 2821 2825 2829 2826 2829 2809 2808 2810 2831 2808 2832 2835 2831 2802 2804 2835 2803 2837 2804 2836 2841 2837 2797 2796 2841 2798 2846 2842 2846 2792 2791 2792 2848 2790 2848 2852 2847 2852 2785 2784 2786 2854 2784 2853 2858 2854 2779 2778 2858 2780 2860 2778 2859 2863 2860 2773 2772 2863 2774 2865 2772 2864 2869 2865 2767 2766 2869 2768 2871 2766 2870 2875 2871 2761 2760 2875 2876 2760 2762 2881 2877 2876 2755 2754 2881 2882 2756 2755 2887 2883 2882 2749 2748 2887 2888 2750 2749 2893 2889 2888 2744 2743 2893 2894 2742 2744 2899 2895 2894 2740 2739 2899 2904 2741 2900 2734 2733 2904 2905 2735 2734 2910 2906 2905 2728 2727 2910 2912 2729 2728 2916 2911 2912 2723 2722 2916 2917 2721 2723 2921 2918 2917 2715 2717 2921 2922 2716 2715 2927 2923 2922 2710 2709 2927 2928 2711 2710 2933 2929 2928 2704 2703 2933 2934 2703 2705 2939 2934 2935 2698 2939 2699 2941 2698 2697 2945 2941 2940 2693 2945 2691 2946 2693 2692 2951 2946 2947 2685 2951 2686 2953 2685 2687 2957 2953 2952 2680 2957 2681 2958 2680 2679 2673 2675 2959 2963 2674 2673 2968 2964 2963 2669 2668 2968 2970 2667 2669 2974 2969 2970 2663 2662 2974 2975 2661 2663 2980 2976 2975 2655 2657 2980 2981 2657 2656 2985 2981 2982 2649 2985 2650 2986 2649 2651 2991 2986 2987 2643 2991 2644 2992 2643 2645 2997 2992 2993 2637 2997 2638 2998 2637 2639 3001 2998 2999 2633 3001 2631 3007 2633 2632 3005 3007 3006 2625 3005 2626 3230 2625 2627 3228 3230 3229 2622 3228 2623 3227 2622 2621 3224 3227 3226 3222 3223 3225 3220 3221 3222 3218 3219 3220 3217 3216 3218 3015 3017 3217 3215 3016 3015 3214 3213 3215 3212 3211 3214 3210 3209 3212 3208 3207 3210 3206 3205 3208 3204 3203 3206 3202 3201 3204 3200 3199 3202 3198 3197 3200 3024 3023 3198 3196 3025 3024 3194 3195 3196 3192 3193 3194 3190 3191 3192 3189 3188 3190 3187 3186 3189 3185 3184 3187 3183 3182 3185 3180 3181 3183 3178 3179 3180 3176 3177 3178 3174 3175 3176 3173 3172 3174 3170 3171 3173 3168 3167 3169 3166 3163 3167 3164 3163 3165 3162 3161 3164 3159 3160 3162 3157 3158 3159 3155 3156 3157 3153 3154 3155 3152 3151 3153 3150 3149 3152 3148 3147 3150 3146 3145 3148 3144 3143 3146 3142 3141 3144 3140 3139 3142 3138 3137 3140 3136 3135 3138 3134 3133 3136 3131 3132 3134 3129 3130 3131 3128 3127 3129 3126 3125 3128 3124 3123 3126 3122 3121 3124 3120 3119 3122 3117 3118 3120 3060 3059 3061 3082 3087 3052 3085 3084 3087 3086 3089 3084 3088 3231 3089 3231 3232 3233 3232 3231 3088 3234 3235 3233 3233 3232 3234 3236 3235 3237 3234 3237 3235 3236 3238 3239 3238 3236 3237 3240 3241 3239 3239 3238 3240 3242 3241 3243 3240 3243 3241 3242 3244 3245 3244 3242 3243 3246 3247 3245 3245 3244 3246 3248 3247 3249 3246 3249 3247 3248 3250 3251 3250 3248 3249 3252 3251 3253 3251 3250 3253 3254 3255 3251 3254 3251 3252 2446 3251 3093 3255 3093 3251

 0.603922 0.647059 0.686275 1 ================================================ FILE: fpv_aggressive_trajectories/resources/rviz/race_track.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /TF1/Frames1 Splitter Ratio: 0.499079198 Tree Height: 401 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: Image Visualization Manager: Class: "" Displays: - Class: rviz/MarkerArray Enabled: true Marker Topic: /track Name: Track Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/rviz_gates Name: Gates Namespaces: "": true Queue Size: 100 Value: true - Class: rviz/Image Enabled: true Image Topic: /image_with_predictions Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /rviz_walls Name: Walls Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Marker Enabled: false Marker Topic: /hummingbird/goal_marker Name: Goal Namespaces: {} Queue Size: 100 Value: false - Class: rviz/Marker Enabled: true Marker Topic: /hummingbird/trajectories_model_based Name: Trajectories Model Based Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Marker Enabled: false Marker Topic: /hummingbird/trajectories_cnn Name: Trajectories CNN Namespaces: {} Queue Size: 100 Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0.0500000007 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /hummingbird/reactive_point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Class: rviz/Marker Enabled: false Marker Topic: /hummingbird/candidate_end_points_center Name: Candidate End Points Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/vehicle_marker Name: Quad Marker Namespaces: vehicle_arm: true vehicle_body: true vehicle_rotor: true Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /hummingbird/debug Name: Current Navigation Goal Namespaces: my_namespace: true Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /hummingbird/global_trajectory Name: Global Trajectory Namespaces: "": true Queue Size: 100 Value: true - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 0; 0; 0 Enabled: true Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 60 Reference Frame: Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: false body: Value: false hummingbird/base_link: Value: false hummingbird/base_link_inertia: Value: false hummingbird/imu_link: Value: false hummingbird/imugt_link: Value: false hummingbird/odometry_sensor1: Value: false hummingbird/odometry_sensor1_link: Value: false hummingbird/odometry_sensorgt_link: Value: false hummingbird/rgb_camera/camera_1_link: Value: false hummingbird/rgb_camera/camera_1_optical_link: Value: false hummingbird/rotor_0: Value: false hummingbird/rotor_1: Value: false hummingbird/rotor_2: Value: false hummingbird/rotor_3: Value: false world: Value: true Marker Scale: 10 Name: TF Show Arrows: false Show Axes: true Show Names: false Tree: world: body: {} hummingbird/base_link: hummingbird/base_link_inertia: {} hummingbird/imu_link: {} hummingbird/imugt_link: {} hummingbird/odometry_sensor1_link: {} hummingbird/odometry_sensorgt_link: {} hummingbird/rgb_camera/camera_1_link: hummingbird/rgb_camera/camera_1_optical_link: {} hummingbird/rotor_0: {} hummingbird/rotor_1: {} hummingbird/rotor_2: {} hummingbird/rotor_3: {} hummingbird/odometry_sensor1: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 50; 50; 50 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 79.2926559 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 17.5849495 Y: 13.6835651 Z: -5.78625298 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 0.894796789 Target Frame: Value: Orbit (rviz) Yaw: 4.03406429 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1056 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000016a00000393fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000025800000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000200000001bb0000001600ffffff000000010000010f000004dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000041fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1855 X: 65 Y: 624 ================================================ FILE: fpv_aggressive_trajectories/resources/rviz/real_world.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.499079198 Tree Height: 389 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.588679016 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: Image Visualization Manager: Class: "" Displays: - Class: rviz/MarkerArray Enabled: true Marker Topic: /track Name: Track Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/rviz_gates Name: Gates Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Image Enabled: true Image Topic: /image_with_predictions Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /rviz_walls Name: Walls Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Marker Enabled: false Marker Topic: /hummingbird/goal_marker Name: Goal Namespaces: {} Queue Size: 100 Value: false - Class: rviz/Marker Enabled: true Marker Topic: /owl/trajectories_model_based Name: Trajectories Model Based Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /owl/trajectories_cnn Name: Trajectories CNN Namespaces: {} Queue Size: 100 Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0.0500000007 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.100000001 Style: Flat Squares Topic: /hummingbird/reactive_point_cloud Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Class: rviz/Marker Enabled: false Marker Topic: /hummingbird/candidate_end_points_center Name: Candidate End Points Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /owl/vehicle_marker Name: Quad Marker Namespaces: vehicle_arm: true vehicle_body: true vehicle_rotor: true Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /owl/debug Name: Current Navigation Goal Namespaces: my_namespace: true Queue Size: 100 Value: true - Class: rviz/Marker Enabled: true Marker Topic: /owl/global_trajectory Name: Global Trajectory Namespaces: {} Queue Size: 100 Value: true - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 0; 0; 0 Enabled: true Line Style: Line Width: 0.0299999993 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 60 Reference Frame: Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.100000001 Class: rviz/Pose Color: 255; 25; 0 Enabled: false Head Length: 0.300000012 Head Radius: 0.100000001 Name: Pose Shaft Length: 1 Shaft Radius: 0.0500000007 Shape: Arrow Topic: /owl/vio_prior Unreliable: false Value: false Enabled: true Global Options: Background Color: 50; 50; 50 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 40.0308342 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 20.2304859 Y: 6.80744839 Z: -6.59393644 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 Pitch: 0.394797593 Target Frame: Value: Orbit (rviz) Yaw: 3.52858806 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1028 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd0000000400000000000002e900000377fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001c6000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001f4000001ab0000001600ffffff000000010000010f000004dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004da000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000041fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004500000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 1855 X: 65 Y: 24 ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/urdf/component_snippets.xacro ================================================ ${namespace} ${bag_file} ${namespace}/base_link ${namespace}/base_link ${rotor_velocity_slowdown_sim} ${wait_to_record_bag} ${frame_rate} ${horizontal_fov} ${image_width} ${image_height} ${image_format} ${min_distance} ${max_distance} gaussian ${noise_mean} ${noise_stddev} ${namespace} true ${frame_rate} camera_${camera_suffix} image_raw camera_info camera_${camera_suffix}_link 0.0 0.0 0.0 0.0 0.0 0.0 0 ${-baseline} 0 0 0 0 ${horizontal_fov} ${image_width} ${image_height} ${image_format} ${min_distance} ${max_distance} gaussian ${noise_mean} ${noise_stddev} ${frame_rate} true 0.0 ${camera_name} image_raw camera_info ${camera_name}/camera_left_link ${baseline_y} 0.0 0.0 0.0 0.0 0.0 ${namespace} command/attitude command/rate command/motor_speed ${imu_sub_topic} gazebo/command/motor_speed ${namespace} ${mavlink_sub_topic} ${imu_sub_topic} ${mavlink_pub_topic} ${motors_speeds_pub_topic} ${gps_update_freq} ${rotor_count} ${reference_magnetic_field_north} ${reference_magnetic_field_east} ${reference_magnetic_field_down} ${reference_latitude} ${reference_longitude} ${reference_altitude} ${namespace} ${namespace}/imu${imu_suffix}_link ${imu_topic} ${gyroscope_noise_density} ${gyroscopoe_random_walk} ${gyroscope_bias_correlation_time} ${gyroscope_turn_on_bias_sigma} ${accelerometer_noise_density} ${accelerometer_random_walk} ${accelerometer_bias_correlation_time} ${accelerometer_turn_on_bias_sigma} ${namespace}/odometry_sensor${odometry_sensor_suffix}_link ${namespace} ${pose_topic} ${pose_with_covariance_topic} ${position_topic} ${transform_topic} ${odometry_topic} ${parent_frame_id} ${child_frame_id} ${measurement_divisor} ${measurement_delay} ${unknown_delay} ${noise_normal_position} ${noise_normal_quaternion} ${noise_normal_linear_velocity} ${noise_normal_angular_velocity} ${noise_uniform_position} ${noise_uniform_quaternion} ${noise_uniform_linear_velocity} ${noise_uniform_angular_velocity} package://rotors_gazebo/resource/${odometry_map} ${image_scale} 0 0 0 0 0 0 0 1 5 0.0 ${horizontal_pos_std_dev} 0.0 0.0 0.0 ${vertical_pos_std_dev} 0.0 0.0 0.0 ${horizontal_vel_std_dev} 0.0 0.0 0.0 ${vertical_vel_std_dev} 0.0 0.0 ${namespace} ${namespace}/gps${gps_suffix}_link ${gps_topic} ${ground_speed_topic} ${horizontal_pos_std_dev} ${vertical_pos_std_dev} ${horizontal_vel_std_dev} ${vertical_vel_std_dev} ${namespace} ${namespace}/magnetometer${magnetometer_suffix}_link ${magnetometer_topic} ${ref_mag_north} ${ref_mag_east} ${ref_mag_down} ${noise_normal} ${noise_uniform_initial_bias} world ${namespace}/base_link ${namespace} ${xyz_offset} ${wind_direction} ${wind_force_mean} ${wind_gust_direction} ${wind_gust_duration} ${wind_gust_start} ${wind_gust_force_mean} true ${frame_rate} 2 L8 160 120 0.04 ${max_range} ${namespace} true 0.11 ${frame_rate} camera_${camera_suffix} camera/image_raw camera/camera_info depth/disparity depth/camera_info depth/points camera_${camera_suffix}_optical_center_link 0.1 50.0 0.0 0.0 0.0 0.0 0.0 ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/urdf/hummingbird.xacro ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/urdf/hummingbird_base.xacro ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/urdf/mav_generic_odometry_sensor.gazebo ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/vehicles/hummingbird.gazebo ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/simulation/vehicles/hummingbird_rgbcamera300200.gazebo ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/urdf/component_snippets.xacro ================================================ ${namespace} ${bag_file} ${namespace}/base_link ${namespace}/base_link ${rotor_velocity_slowdown_sim} ${wait_to_record_bag} ${frame_rate} ${horizontal_fov} ${image_width} ${image_height} ${image_format} ${min_distance} ${max_distance} gaussian ${noise_mean} ${noise_stddev} ${namespace} true ${frame_rate} camera_${camera_suffix} image_raw camera_info camera_${camera_suffix}_link 0.0 0.0 0.0 0.0 0.0 0.0 ${frame_rate} ${horizontal_fov} ${image_width} ${image_height} ${image_format} ${min_distance} ${max_distance} gaussian ${noise_mean} ${noise_stddev} ${namespace} true ${frame_rate} camera_${camera_suffix} image_raw camera_info camera_${camera_suffix}_link 0.0 0.0 0.0 0.0 0.0 0.0 0 ${-baseline} 0 0 0 0 ${horizontal_fov} ${image_width} ${image_height} ${image_format} ${min_distance} ${max_distance} gaussian ${noise_mean} ${noise_stddev} ${frame_rate} true 0.0 ${camera_name} image_raw camera_info ${camera_name}/camera_left_link ${baseline_y} 0.0 0.0 0.0 0.0 0.0 ${namespace} command/attitude command/rate command/motor_speed ${imu_sub_topic} gazebo/command/motor_speed ${namespace} ${mavlink_sub_topic} ${imu_sub_topic} ${mavlink_pub_topic} ${motors_speeds_pub_topic} ${gps_update_freq} ${rotor_count} ${reference_magnetic_field_north} ${reference_magnetic_field_east} ${reference_magnetic_field_down} ${reference_latitude} ${reference_longitude} ${reference_altitude} ${namespace} ${namespace}/imu${imu_suffix}_link ${imu_topic} ${gyroscope_noise_density} ${gyroscope_random_walk} ${gyroscope_bias_correlation_time} ${gyroscope_turn_on_bias_sigma} ${accelerometer_noise_density} ${accelerometer_random_walk} ${accelerometer_bias_correlation_time} ${accelerometer_turn_on_bias_sigma} ${namespace}/odometry_sensor${odometry_sensor_suffix}_link ${namespace} ${pose_topic} ${pose_with_covariance_topic} ${position_topic} ${transform_topic} ${odometry_topic} ${parent_frame_id} ${child_frame_id} ${measurement_divisor} ${measurement_delay} ${unknown_delay} ${noise_normal_position} ${noise_normal_quaternion} ${noise_normal_linear_velocity} ${noise_normal_angular_velocity} ${noise_uniform_position} ${noise_uniform_quaternion} ${noise_uniform_linear_velocity} ${noise_uniform_angular_velocity} package://rotors_gazebo/resource/${odometry_map} ${image_scale} 0 0 0 0 0 0 0 1 5 0.0 ${horizontal_pos_std_dev} 0.0 0.0 0.0 ${vertical_pos_std_dev} 0.0 0.0 0.0 ${horizontal_vel_std_dev} 0.0 0.0 0.0 ${vertical_vel_std_dev} 0.0 0.0 ${namespace} ${namespace}/gps${gps_suffix}_link ${gps_topic} ${ground_speed_topic} ${horizontal_pos_std_dev} ${vertical_pos_std_dev} ${horizontal_vel_std_dev} ${vertical_vel_std_dev} ${namespace} ${namespace}/magnetometer${magnetometer_suffix}_link ${magnetometer_topic} ${ref_mag_north} ${ref_mag_east} ${ref_mag_down} ${noise_normal} ${noise_uniform_initial_bias} ${namespace} ${namespace}/pressure_sensor${pressure_sensor_suffix}_link ${pressure_topic} ${reference_altitude} ${pressure_variance} world ${namespace}/base_link ${namespace} ${xyz_offset} ${wind_direction} ${wind_force_mean} ${wind_gust_direction} ${wind_gust_duration} ${wind_gust_start} ${wind_gust_force_mean} ${wind_speed_mean} ${use_custom_static_wind_field} ${custom_wind_field_path} true ${frame_rate} 2 L8 640 480 0.01 ${max_range} ${namespace} true 0.11 ${frame_rate} camera_${camera_suffix} camera/image_raw camera/camera_info depth/disparity depth/camera_info depth/points camera_${camera_suffix}_optical_center_link 0.5 ${max_range} 0.0 0.0 0.0 0.0 0.0 ================================================ FILE: fpv_aggressive_trajectories/resources/urdf/hummingbird_base.xacro ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/urdf/mav_generic_odometry_sensor_rgb.gazebo ================================================ ================================================ FILE: fpv_aggressive_trajectories/resources/worlds/controller_learning.world ================================================ 0.001 1000 1.0 model://sun -20.0 10 20.0 0 0.5 0 orbit -60.0 -60.0 0 0 0 0 true 0 0 1 120 120 100 50 false 0 0 1 120 120 -60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 -60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 0 0 0 0 1510043711 945147391 0 -34.95 -11.3618 0 0 -0 0 1 1 1 -34.95 -11.3618 0 0 -0 0 0 0 0 0 -0 0 0 0 0 0 -0 0 0 0 0 0 -0 0 2.0 2.0 0 0 0 -3.12489 1 1 1 2.0 2.0 0 0 0 -3.12489 0 0 0 0 -0 0 0 0 0 0 -0 0 0 0 0 0 -0 0 0 0 0 0 -0 0 1 1 1 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 10 0 -0 0 ================================================ FILE: fpv_aggressive_trajectories/resources/worlds/random_gates.world ================================================ 0.001 1000 model://sun -20.0 10 20.0 0 0.5 0 orbit -60.0 -60.0 0 0 0 0 true 0 0 1 120 120 100 50 false 0 0 1 120 120 -60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 -60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 5 30 0.0 0 0 0.0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0.0 30 4.4 0 0 0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0 30 1.4 0 0 0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0.7 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 130.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 135.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 140.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 145.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 150.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 155.0 30 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0 0 0 0 1510043711 945147391 0 0 0 0 0 -0 0 1 1 1 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 10 0 -0 0 ================================================ FILE: fpv_aggressive_trajectories/resources/worlds/single_gate.world ================================================ 0.001 1000 model://sun -20.0 10 20.0 0 0.5 0 orbit -60.0 -60.0 0 0 0 0 true 0 0 1 120 120 100 50 false 0 0 1 120 120 -60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 -60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 4.0 0.0 1.0 0.0 0.0 0.0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0 0 0 0 1510043711 945147391 0 0 0 0 0 -0 0 1 1 1 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 10 0 -0 0 ================================================ FILE: fpv_aggressive_trajectories/resources/worlds/track.world ================================================ model://sun -20.0 10 20.0 0 0.5 0 orbit -60.0 -60.0 0 0 0 0 true 0 0 1 120 120 100 50 false 0 0 1 120 120 -60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 -60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 60.0 0.0 34.0 0 0 0 true 1.0 0.1 120.0 70.0 0.1 120.0 70.0 0.0 60.0 34.0 0 0 0 true 1.0 120 0.1 70.0 120 0.1 70.0 10 0 0.3 0 0 0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 10.7 0 4.4 0 0 0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 20 0 1.4 0 0 0 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 44.7 1.4 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 44.7 1.4 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 44.7 1.4 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 44.7 1.4 1.4 0 0 0.8 true model://race_track/real_world/gate/meshes/gate.dae model://race_track/real_world/gate/meshes/gate.dae 0 0 0 0 1510043711 945147391 0 0 0 0 0 -0 0 1 1 1 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 10 0 -0 0 ================================================ FILE: fpv_aggressive_trajectories/rviz/trajectory_comparison_looping_sim.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Actual Quadrotor Pose1/Shape1 - /Reference State1/Shape1 - /MPCReference1/Shape1 - /VIO Raw1/Shape1 - /Image1 Splitter Ratio: 0.6024734973907471 Tree Height: 554 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Experimental: false Name: Time SyncMode: 0 SyncSource: Image Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 100 Reference Frame: Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /hummingbird/autopilot_feedback_trajectory_ref Name: Reference Trajectory Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/autopilot_feedback_trajectory_se Name: Executed Trajectory Namespaces: {} Queue Size: 100 Value: true - Class: rviz/Axes Enabled: false Length: 0.5 Name: Origin Radius: 0.05000000074505806 Reference Frame: Value: false - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Actual Quadrotor Pose Position Tolerance: 0 Shape: Alpha: 1 Axes Length: 0.20000000298023224 Axes Radius: 0.05000000074505806 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /hummingbird/ground_truth/odometry Unreliable: false Value: true - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: Reference State Position Tolerance: 0 Shape: Alpha: 1 Axes Length: 0.4000000059604645 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /hummingbird/reference_odometry Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 252; 233; 79 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: MPC Path VIO Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /hummingbird/vio_mpc_path Unreliable: false Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: MPC Path GT Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /hummingbird/gt_mpc_path Unreliable: false Value: false - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: MPCReference Position Tolerance: 0 Shape: Alpha: 1 Axes Length: 0.10000000149011612 Axes Radius: 0.02500000037252903 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /hummingbird/mpc/reference_state Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /hummingbird/reference_trajectory Name: Autopilot Reference Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/fpv_reference_trajectory Name: Planned Trajectory Namespaces: {} Queue Size: 100 Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: VIO Raw Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /vins_estimator/imu_propagate Unreliable: false Value: false - Class: rviz/Image Enabled: true Image Topic: /feature_tracker/feature_img Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: State Estimate Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /hummingbird/odometry_converted Unreliable: false Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /hummingbird/gt_state Name: Ground Truth Trajectory Namespaces: {} Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: false Marker Topic: /hummingbird/feedforward_bodyrates_viz Name: Feedforward Bodyrates Namespaces: {} Queue Size: 100 Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /hummingbird/quadrotor_viz Name: Quadrotor Namespaces: {} Queue Size: 100 Value: false Enabled: true Global Options: Background Color: 0; 0; 0 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 7.393194675445557 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 2.8953537940979004 Y: -0.8881855010986328 Z: -0.7229487895965576 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.24479827284812927 Target Frame: follower_camera_target Value: ThirdPersonFollower (rviz) Yaw: 1.9748201370239258 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1440 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000018400000506fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b3000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002f40000024d0000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000008760000050600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: true Width: 2560 X: 3840 Y: 0 ================================================ FILE: fpv_aggressive_trajectories/src/fpv_aggressive_trajectories.cpp ================================================ #include "fpv_aggressive_trajectories/fpv_aggressive_trajectories.h" #include #include #include #include #include #include #include #include "acrobatic_trajectory_helper/acrobatic_sequence.h" namespace fpv_aggressive_trajectories { FPVAggressiveTrajectories::FPVAggressiveTrajectories(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) : nh_(nh), pnh_(pnh), state_predictor_(nh_, pnh_), state_predictor_gt_(nh_, pnh_) { if (!loadParameters()) { ROS_ERROR("[%s] Failed to load all parameters", ros::this_node::getName().c_str()); ros::shutdown(); } base_controller_params_.loadParameters(nh_); base_controller_params_gt_.loadParameters(nh_); visualizer_ = std::make_shared( fpv_aggressive_trajectories::Visualizer(nh_, pnh_)); toggle_experiment_sub_ = nh_.subscribe("fpv_quad_looping/execute_trajectory", 1, &FPVAggressiveTrajectories::startExecutionCallback, this); odometry_sub_ = nh_.subscribe("state_estimate", 1, &FPVAggressiveTrajectories::odometryCallback, this, ros::TransportHints().tcpNoDelay()); odometry_gt_sub_ = nh_.subscribe("ground_truth/odometry", 1, &FPVAggressiveTrajectories::odometryGTCallback, this, ros::TransportHints().tcpNoDelay()); control_command_pub_ = nh_.advertise( "autopilot/control_command_input", 1); control_command_gt_pub_ = nh_.advertise( "/hummingbird/control_command_gt", 1); marker_pub_ref_ = nh_.advertise("reference_trajectory", 1); aggressive_end_pub_ = nh_.advertise("switch_to_network", 1); vio_ref_pub_ = nh_.advertise("vio_reference", 1); traj_comp_finish_pub_ = nh_.advertise("trajectory_computation_finish", 1); } FPVAggressiveTrajectories::~FPVAggressiveTrajectories() {} quadrotor_common::QuadStateEstimate FPVAggressiveTrajectories::getPredictedStateEstimate( const ros::Time& time, const state_predictor::StatePredictor* predictor) { return predictor->predictState(time); } void FPVAggressiveTrajectories::startExecutionCallback( const std_msgs::BoolConstPtr& msg) { ROS_INFO("Received startExecutionCallback message!"); if (msg->data) { // publish the reference of the current state already before maneuver // computation quadrotor_common::QuadStateEstimate odom_start_computation = received_state_est_; quadrotor_common::TrajectoryPoint start_state; start_state.position = odom_start_computation.position; start_state.velocity = odom_start_computation.velocity; start_state.orientation = odom_start_computation.orientation; quadrotor_msgs::TrajectoryPoint ref_msg = start_state.toRosMessage(); vio_ref_pub_.publish(ref_msg); computeManeuver(); } if (!msg->data) { ROS_INFO("Restarting experiment, going to OFF state!"); state_machine_ = STATES::kOff; } } void FPVAggressiveTrajectories::computeManeuver() { ros::Time time_start_computation = ros::Time::now(); trajectory_queue_.clear(); trajectory_queue_gt_.clear(); sent_maneuver_end_msg_ = false; quadrotor_common::QuadStateEstimate odom_start_computation = received_state_est_; quadrotor_common::QuadStateEstimate odom_start_computation_gt = received_state_est_gt_; nav_msgs::Odometry odom_gt_start_computation = odometry_gt_; quadrotor_common::TrajectoryPoint start_state; start_state.position = odom_start_computation.position; start_state.velocity = odom_start_computation.velocity; quadrotor_common::TrajectoryPoint start_state_gt; start_state_gt.position = odom_start_computation_gt.position; start_state_gt.velocity = odom_start_computation_gt.velocity; AcrobaticSequence acrobatic_sequence(start_state); AcrobaticSequence acrobatic_sequence_gt(start_state_gt); bool success = true; Eigen::Vector3d offset_circle_from_start = Eigen::Vector3d(4.0, 0.0, 0.0); Eigen::Vector3d offset_circle_from_end = Eigen::Vector3d(-1.0, 0.0, 1.5); // standard loop success = success && acrobatic_sequence.appendLoops( 1, 4.5, 1.5, offset_circle_from_start, offset_circle_from_end, true, traj_sampling_freq_); // matty loop // success = success && acrobatic_sequence.appendMattyLoop(1, 4.5, 1.5, offset_circle_from_start, offset_circle_from_end); // barrel roll // success = success && acrobatic_sequence.appendBarrelRoll(1, 4.5, 1.5, offset_circle_from_start, offset_circle_from_end, true); visualizer_->visualizeTrajectories(acrobatic_sequence.getManeuverList()); if (success) { for (const quadrotor_common::Trajectory& trajectory : acrobatic_sequence.getManeuverList()) { trajectory_queue_.push_back(trajectory); } for (const quadrotor_common::Trajectory& trajectory : acrobatic_sequence_gt.getManeuverList()) { trajectory_queue_gt_.push_back(trajectory); } state_machine_ = STATES::kExecuteTrajectory; first_time_in_new_state_ = true; ROS_INFO("Maneuver computation successful!"); } else { ROS_ERROR("Maneuver computation failed! Will not execute trajectory."); } std_msgs::Bool bool_msg; bool_msg.data = success; traj_comp_finish_pub_.publish(bool_msg); ROS_INFO("Maneuver computation took %.4f seconds.", (ros::Time::now() - time_start_computation).toSec()); } void FPVAggressiveTrajectories::odometryCallback( const nav_msgs::OdometryConstPtr& msg) { received_state_est_ = quadrotor_common::QuadStateEstimate(*msg); received_state_est_gt_ = quadrotor_common::QuadStateEstimate(odometry_gt_); // both velocity estimates are expressed in bodyframe received_state_est_.transformVelocityToWorldFrame(); received_state_est_gt_.transformVelocityToWorldFrame(); // Push received state estimate into predictor state_predictor_.updateWithStateEstimate(received_state_est_); state_predictor_gt_.updateWithStateEstimate(received_state_est_gt_); if (state_machine_ == STATES::kExecuteTrajectory) { quadrotor_common::ControlCommand control_cmd; quadrotor_common::ControlCommand control_cmd_gt; ros::Time wall_time_now = ros::Time::now(); double control_command_delay = 0.05; // TODO: read from parameter file ros::Time command_execution_time = wall_time_now + ros::Duration(control_command_delay); quadrotor_common::QuadStateEstimate predicted_state = getPredictedStateEstimate(command_execution_time, &state_predictor_); quadrotor_common::QuadStateEstimate predicted_state_gt = getPredictedStateEstimate(command_execution_time, &state_predictor_gt_); ros::Duration trajectory_execution_left_duration(0.0); int trajectories_left_in_queue = 0; ros::Duration trajectory_execution_left_duration_gt(0.0); const ros::Time start_control_command_computation = ros::Time::now(); control_cmd = computeControlCommand(predicted_state, &trajectory_execution_left_duration, &trajectories_left_in_queue); control_cmd_gt = control_cmd; control_cmd_gt.collective_thrust = 0.0; control_cmd.timestamp = wall_time_now; control_cmd.expected_execution_time = command_execution_time; const ros::Duration control_computation_time = ros::Time::now() - start_control_command_computation; publishControlCommand(control_cmd, control_cmd_gt); } else if (state_machine_ == STATES::kNetworkControl) { // we publish some dummy message here to prevent autopilot from going into // HOVER quadrotor_common::ControlCommand control_cmd; control_cmd.armed = true; control_cmd.collective_thrust = 5.0; // on purpose low thrust value to detect if this control command // was executed control_cmd.bodyrates = Eigen::Vector3d::Zero(); control_cmd.control_mode = quadrotor_common::ControlMode::BODY_RATES; quadrotor_common::ControlCommand control_cmd_gt = control_cmd; publishControlCommand(control_cmd, control_cmd_gt); } } void FPVAggressiveTrajectories::odometryGTCallback( const nav_msgs::OdometryConstPtr& msg) { odometry_gt_ = *msg; visualizer_->displayQuadrotor(); } quadrotor_common::ControlCommand FPVAggressiveTrajectories::computeControlCommand( const quadrotor_common::QuadStateEstimate& state_estimate, ros::Duration* trajectory_execution_left_duration, int* trajectories_left_in_queue) { quadrotor_common::TrajectoryPoint reference_state_; quadrotor_common::Trajectory reference_trajectory_; ros::Time time_now = ros::Time::now(); if (first_time_in_new_state_) { first_time_in_new_state_ = false; time_start_trajectory_execution_ = time_now; } if (trajectory_queue_.empty()) { ROS_ERROR("[%s] Trajectory queue was unexpectedly emptied!", pnh_.getNamespace().c_str()); *trajectory_execution_left_duration = ros::Duration(0.0); *trajectories_left_in_queue = 0; reference_trajectory_ = quadrotor_common::Trajectory(reference_state_); quadrotor_msgs::TrajectoryPoint ref_msg = reference_trajectory_.points.front().toRosMessage(); vio_ref_pub_.publish(ref_msg); return base_controller_.run(state_estimate, reference_trajectory_, base_controller_params_); } // enable the NW a bit early double a_bit_early = 0.0; if (trajectory_queue_.size() == 1 && !sent_maneuver_end_msg_ && (time_now - time_start_trajectory_execution_ + ros::Duration(a_bit_early)) > trajectory_queue_.front().points.back().time_from_start) { ROS_INFO("Enable network!"); sent_maneuver_end_msg_ = true; std_msgs::Bool bool_msg; bool_msg.data = true; aggressive_end_pub_.publish(bool_msg); } if ((time_now - time_start_trajectory_execution_) > trajectory_queue_.front().points.back().time_from_start) { if (trajectory_queue_.size() == 1) { ROS_WARN("This was the last trajectory! t = %.4f\n", (time_now - time_start_trajectory_execution_).toSec()); // This was the last trajectory in the queue -> go back to hover reference_state_ = trajectory_queue_.back().points.back(); *trajectory_execution_left_duration = ros::Duration(0.0); *trajectories_left_in_queue = 0; trajectory_queue_.pop_front(); state_machine_ = STATES::kAutopilot; // kNetworkControl; reference_trajectory_ = quadrotor_common::Trajectory(reference_state_); quadrotor_msgs::TrajectoryPoint ref_msg = reference_trajectory_.points.front().toRosMessage(); vio_ref_pub_.publish(ref_msg); return base_controller_.run(state_estimate, reference_trajectory_, base_controller_params_); } else { time_start_trajectory_execution_ += trajectory_queue_.front().points.back().time_from_start; trajectory_queue_.pop_front(); } } // Time from trajectory start and corresponding reference state. const ros::Duration dt = time_now - time_start_trajectory_execution_; reference_state_ = trajectory_queue_.front().getStateAtTime(dt); // New trajectory where we fill in our lookahead horizon. reference_trajectory_ = quadrotor_common::Trajectory(); reference_trajectory_.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL; bool lookahead_reached(false); // Boolean break flag // Time wrap if lookahead spans multiple trajectories: double time_wrapover(0.0); for (auto trajectory : trajectory_queue_) { for (auto point : trajectory.points) { // Check wether we reached our lookahead. // Use boolean flag to also break the outer loop. if (point.time_from_start.toSec() > (dt.toSec() - time_wrapover + predictive_control_lookahead_)) { lookahead_reached = true; break; } // Add a point if the time corresponds to a sample on the lookahead. if (point.time_from_start.toSec() > (dt.toSec() - time_wrapover)) { // check if two trajectory points are the same... if (reference_trajectory_.points.size() > 1) { point.time_from_start += ros::Duration(time_wrapover); reference_trajectory_.points.push_back(point); } else { // this is the first point of the reference trajectory reference_trajectory_.points.push_back(point); } } } if (lookahead_reached) break; // Break on boolean flag. // Sum up the wrap-over time if lookahead spans multiple trajectories. time_wrapover += trajectory.points.back().time_from_start.toSec(); } *trajectory_execution_left_duration = trajectory_queue_.front().points.back().time_from_start - reference_state_.time_from_start; if (trajectory_queue_.size() > 1) { std::list::const_iterator it; for (it = std::next(trajectory_queue_.begin(), 1); it != trajectory_queue_.end(); it++) { *trajectory_execution_left_duration += it->points.back().time_from_start; } } *trajectories_left_in_queue = trajectory_queue_.size(); // handle case of empty reference_trajectory if (reference_trajectory_.points.empty()) { ROS_WARN("Empty reference trajectory!"); *trajectory_execution_left_duration = ros::Duration(0.0); *trajectories_left_in_queue = 0; reference_trajectory_ = quadrotor_common::Trajectory(reference_state_); quadrotor_msgs::TrajectoryPoint ref_msg = reference_trajectory_.points.front().toRosMessage(); vio_ref_pub_.publish(ref_msg); return base_controller_.run(state_estimate, reference_trajectory_, base_controller_params_); } // visualize reference trajectory visualizer_->visualizeReferenceTrajectory(&reference_trajectory_); quadrotor_msgs::TrajectoryPoint ref_msg = reference_trajectory_.points.front().toRosMessage(); vio_ref_pub_.publish(ref_msg); quadrotor_common::ControlCommand ctrl_cmd = base_controller_.run( state_estimate, reference_trajectory_, base_controller_params_); return ctrl_cmd; } void FPVAggressiveTrajectories::publishControlCommand( const quadrotor_common::ControlCommand& control_command, const quadrotor_common::ControlCommand& control_command_gt) { if (state_machine_ == STATES::kOff) { return; } quadrotor_msgs::ControlCommand control_cmd_msg; control_cmd_msg = control_command.toRosMessage(); control_command_pub_.publish(control_cmd_msg); state_predictor_.pushCommandToQueue(control_command); } bool FPVAggressiveTrajectories::loadParameters() { if (!quadrotor_common::getParam("desired_yaw_P", desired_heading_, 0.0)) return false; if (!quadrotor_common::getParam("loop_rate", exec_loop_rate_, 55.0)) return false; if (!quadrotor_common::getParam("circle_velocity", circle_velocity_, 1.0)) return false; if (!quadrotor_common::getParam("n_loops", n_loops_, 1)) return false; return true; } } // namespace fpv_aggressive_trajectories int main(int argc, char** argv) { ros::init(argc, argv, "fpv_aggressive_trajectories"); fpv_aggressive_trajectories::FPVAggressiveTrajectories fpv_aggressive_trajectories; ros::MultiThreadedSpinner spinner(2); spinner.spin(); return 0; } ================================================ FILE: fpv_aggressive_trajectories/src/odometry_republisher.cpp ================================================ #include "fpv_aggressive_trajectories/odometry_republisher.h" #include namespace odometry_republisher { OdometryRepublisher::OdometryRepublisher() { Eigen::Quaterniond q_B_S = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); Eigen::Vector3d t_B_S = Eigen::Vector3d(0.2, 0.0, 0.0); T_B_S_ = rpg::Pose(q_B_S, t_B_S); state_estimate_sub_ = nh_.subscribe("odometry_in", 1, &OdometryRepublisher::odometryInCallback, this, ros::TransportHints().tcpNoDelay()); odometry_out_pub_ = nh_.advertise("odometry_out", 1); } OdometryRepublisher::~OdometryRepublisher() {} void OdometryRepublisher::odometryInCallback( const nav_msgs::OdometryConstPtr& msg) { nav_msgs::Odometry odometry_out_msg; odometry_out_msg.header = msg->header; transformOdometry(msg, &odometry_out_msg); odometry_out_msg.header.frame_id = "world"; odometry_out_pub_.publish(odometry_out_msg); } void OdometryRepublisher::transformOdometry( const nav_msgs::OdometryConstPtr& odom_in, nav_msgs::Odometry* odom_out) { // transform odometry estimate to body frame of quadrotor Eigen::Quaterniond q_W_S = Eigen::Quaterniond( odom_in->pose.pose.orientation.w, odom_in->pose.pose.orientation.x, odom_in->pose.pose.orientation.y, odom_in->pose.pose.orientation.z); q_W_S.normalize(); Eigen::Vector3d t_W_S = Eigen::Vector3d(odom_in->pose.pose.position.x, odom_in->pose.pose.position.y, odom_in->pose.pose.position.z); rpg::Pose T_W_S = rpg::Pose(q_W_S, t_W_S); // transform position & orientation rpg::Pose T_W_B = T_W_S * T_B_S_.inverse(); // transform angular velocity Eigen::Vector3d omega_body = T_B_S_.inverse().getEigenQuaternion() * Eigen::Vector3d(odom_in->twist.twist.angular.x, odom_in->twist.twist.angular.y, odom_in->twist.twist.angular.z); // transform linear velocity Eigen::Vector3d vlin_world = T_B_S_.inverse().getEigenQuaternion() * Eigen::Vector3d(odom_in->twist.twist.linear.x, odom_in->twist.twist.linear.y, odom_in->twist.twist.linear.z) + omega_body.cross(-T_B_S_.getPosition()); Eigen::Vector3d vlin_body = q_W_S.inverse() * vlin_world; odom_out->pose.pose.position.x = T_W_B.getPosition().x(); odom_out->pose.pose.position.y = T_W_B.getPosition().y(); odom_out->pose.pose.position.z = T_W_B.getPosition().z(); odom_out->pose.pose.orientation.w = T_W_B.getRotation().w(); odom_out->pose.pose.orientation.x = T_W_B.getRotation().x(); odom_out->pose.pose.orientation.y = T_W_B.getRotation().y(); odom_out->pose.pose.orientation.z = T_W_B.getRotation().z(); odom_out->twist.twist.linear.x = vlin_world.x(); odom_out->twist.twist.linear.y = vlin_world.y(); odom_out->twist.twist.linear.z = vlin_world.z(); odom_out->twist.twist.angular.x = omega_body.x(); odom_out->twist.twist.angular.y = omega_body.y(); odom_out->twist.twist.angular.z = omega_body.z(); } } // namespace odometry_republisher int main(int argc, char** argv) { ros::init(argc, argv, "odometry_republisher"); odometry_republisher::OdometryRepublisher odometry_republisher; ros::spin(); return 0; } ================================================ FILE: fpv_aggressive_trajectories/src/visualize.cpp ================================================ #include "fpv_aggressive_trajectories/visualize.h" #include #include namespace fpv_aggressive_trajectories { Visualizer::Visualizer(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) : nh_(nh), pnh_(pnh) { printf("Initiated visualizer\n"); int num_rotors = 4; float arm_len = 0.2; float body_width = 0.2; float body_height = 0.1; create_vehicle_markers(num_rotors, arm_len, body_width, body_height); bodyrates_viz_pub_ = nh_.advertise( "feedforward_bodyrates_viz", 1); marker_ref_pub_ = nh_.advertise("reference_trajectory", 1); marker_pub_ = nh_.advertise( "fpv_reference_trajectory", 1); vehicle_marker_pub_ = nh_.advertise("quadrotor_viz", 1); } Visualizer::~Visualizer() {} void Visualizer::visualizeReferenceTrajectory( const quadrotor_common::Trajectory* trajectory) { // Reference trajectory visualization_msgs::MarkerArray marker_msg_ref; visualization_msgs::Marker marker; marker.header.frame_id = "world"; marker.header.stamp = ros::Time::now(); marker.ns = ""; marker.action = visualization_msgs::Marker::MODIFY; marker.lifetime = ros::Duration(0); marker.type = visualization_msgs::Marker::LINE_STRIP; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; marker.pose.position.z = 0.0; marker.pose.orientation.w = 1.0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.id = 0; marker.scale.x = 0.015; marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0; for (auto it = trajectory->points.begin(); it != trajectory->points.end(); it++) { geometry_msgs::Point point; point.x = it->position.x(); point.y = it->position.y(); point.z = it->position.z(); marker.points.push_back(point); } marker_msg_ref.markers.push_back(marker); marker_ref_pub_.publish(marker_msg_ref); } void Visualizer::visualizeTrajectories( const std::list& maneuver_list) { visualization_msgs::MarkerArray markers; int marker_id = 0; double marker_lifetime = 300.0; for (auto trajectory : maneuver_list) { for (auto point : trajectory.points) { Eigen::Quaternion q_pose = Eigen::Quaterniond(point.orientation.w(), point.orientation.x(), point.orientation.y(), point.orientation.z()); Eigen::Quaternion q_heading = Eigen::Quaternion(Eigen::AngleAxis( point.heading, Eigen::Matrix::UnitZ())); Eigen::Quaternion q_orientation = q_pose * q_heading; // publish marker of reference at this point visualization_msgs::Marker z_axis; z_axis.header.frame_id = "world"; z_axis.ns = ""; z_axis.lifetime = ros::Duration(marker_lifetime); z_axis.header.stamp = ros::Time(); z_axis.type = visualization_msgs::Marker::Type::CYLINDER; z_axis.id = marker_id; Eigen::Vector3d offset_body = Eigen::Vector3d(0.0, 0.0, 0.1); Eigen::Vector3d offset_world = q_orientation * offset_body; Eigen::Vector3d pos_corr = point.position + offset_world; // set off position by half the marker size z_axis.pose.position.x = pos_corr.x(); z_axis.pose.position.y = pos_corr.y(); z_axis.pose.position.z = pos_corr.z(); z_axis.pose.orientation.w = q_orientation.w(); z_axis.pose.orientation.x = q_orientation.x(); z_axis.pose.orientation.y = q_orientation.y(); z_axis.pose.orientation.z = q_orientation.z(); z_axis.scale.x = 0.05; z_axis.scale.y = 0.05; z_axis.scale.z = 0.2; z_axis.color.a = 1.0; z_axis.color.r = 0.0; z_axis.color.g = 0.0; z_axis.color.b = 1.0; markers.markers.push_back(z_axis); // rotate to get x-axis Eigen::Quaternion q_z_x = Eigen::Quaternion(Eigen::AngleAxis( M_PI / 2.0, Eigen::Matrix::UnitY())); Eigen::Quaternion q_x = q_orientation * q_z_x; visualization_msgs::Marker x_axis; x_axis.header.frame_id = "world"; x_axis.ns = ""; x_axis.lifetime = ros::Duration(marker_lifetime); x_axis.header.stamp = ros::Time(); x_axis.type = visualization_msgs::Marker::Type::CYLINDER; x_axis.id = marker_id + 1; offset_body = Eigen::Vector3d(0.0, 0.0, 0.1); offset_world = q_x * offset_body; pos_corr = point.position + offset_world; x_axis.pose.position.x = pos_corr.x(); x_axis.pose.position.y = pos_corr.y(); x_axis.pose.position.z = pos_corr.z(); x_axis.pose.orientation.w = q_x.w(); x_axis.pose.orientation.x = q_x.x(); x_axis.pose.orientation.y = q_x.y(); x_axis.pose.orientation.z = q_x.z(); x_axis.scale.x = 0.05; x_axis.scale.y = 0.05; x_axis.scale.z = 0.2; x_axis.color.a = 1.0; x_axis.color.r = 1.0; x_axis.color.g = 0.0; x_axis.color.b = 0.0; markers.markers.push_back(x_axis); // rotate to get y-axis Eigen::Quaternion q_z_y = Eigen::Quaternion(Eigen::AngleAxis( -M_PI / 2.0, Eigen::Matrix::UnitX())); Eigen::Quaternion q_y = q_orientation * q_z_y; visualization_msgs::Marker y_axis; y_axis.header.frame_id = "world"; y_axis.ns = ""; y_axis.lifetime = ros::Duration(marker_lifetime); y_axis.header.stamp = ros::Time(); y_axis.type = visualization_msgs::Marker::Type::CYLINDER; y_axis.id = marker_id + 2; offset_body = Eigen::Vector3d(0.0, 0.0, 0.1); offset_world = q_y * offset_body; pos_corr = point.position + offset_world; y_axis.pose.position.x = pos_corr.x(); y_axis.pose.position.y = pos_corr.y(); y_axis.pose.position.z = pos_corr.z(); y_axis.pose.orientation.w = q_y.w(); y_axis.pose.orientation.x = q_y.x(); y_axis.pose.orientation.y = q_y.y(); y_axis.pose.orientation.z = q_y.z(); y_axis.scale.x = 0.05; y_axis.scale.y = 0.05; y_axis.scale.z = 0.2; y_axis.color.a = 1.0; y_axis.color.r = 0.0; y_axis.color.g = 1.0; y_axis.color.b = 0.0; markers.markers.push_back(y_axis); marker_id += 3; } } marker_pub_.publish(markers); } void Visualizer::displayQuadrotor() { vehicle_marker_pub_.publish(*vehicle_marker_); } void Visualizer::create_vehicle_markers(int num_rotors, float arm_len, float body_width, float body_height) { if (num_rotors <= 0) num_rotors = 2; if (vehicle_marker_) return; double marker_scale_ = 1.0; vehicle_marker_ = std::make_shared(); vehicle_marker_->markers.reserve(2 * num_rotors + 1); // rotor marker template visualization_msgs::Marker rotor; rotor.header.stamp = ros::Time(); rotor.header.frame_id = "/hummingbird/base_link"; rotor.ns = "vehicle_rotor"; rotor.action = visualization_msgs::Marker::ADD; rotor.type = visualization_msgs::Marker::CYLINDER; rotor.scale.x = 0.2 * marker_scale_; rotor.scale.y = 0.2 * marker_scale_; rotor.scale.z = 0.01 * marker_scale_; rotor.color.r = 0.4; rotor.color.g = 0.4; rotor.color.b = 0.4; rotor.color.a = 0.8; rotor.pose.position.z = 0; // arm marker template visualization_msgs::Marker arm; arm.header.stamp = ros::Time(); arm.header.frame_id = "/hummingbird/base_link"; arm.ns = "vehicle_arm"; arm.action = visualization_msgs::Marker::ADD; arm.type = visualization_msgs::Marker::CUBE; arm.scale.x = arm_len * marker_scale_; arm.scale.y = 0.02 * marker_scale_; arm.scale.z = 0.01 * marker_scale_; arm.color.r = 0.0; arm.color.g = 0.0; arm.color.b = 1.0; arm.color.a = 1.0; arm.pose.position.z = -0.015 * marker_scale_; float angle_increment = 2 * M_PI / num_rotors; for (float angle = angle_increment / 2; angle <= (2 * M_PI); angle += angle_increment) { rotor.pose.position.x = arm_len * cos(angle) * marker_scale_; rotor.pose.position.y = arm_len * sin(angle) * marker_scale_; rotor.id++; arm.pose.position.x = rotor.pose.position.x / 2; arm.pose.position.y = rotor.pose.position.y / 2; arm.pose.orientation = tf::createQuaternionMsgFromYaw(angle); arm.id++; vehicle_marker_->markers.push_back(rotor); vehicle_marker_->markers.push_back(arm); } // body marker template visualization_msgs::Marker body; body.header.stamp = ros::Time(); body.header.frame_id = "/hummingbird/base_link"; body.ns = "vehicle_body"; body.action = visualization_msgs::Marker::ADD; body.type = visualization_msgs::Marker::CUBE; body.scale.x = body_width * marker_scale_; body.scale.y = body_width * marker_scale_; body.scale.z = body_height * marker_scale_; body.color.r = 0.0; body.color.g = 1.0; body.color.b = 0.0; body.color.a = 0.8; vehicle_marker_->markers.push_back(body); } } // namespace fpv_aggressive_trajectories ================================================ FILE: odometry_converter/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(odometry_converter) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) cs_add_library(odometry_converter src/odometry_converter.cpp) cs_add_executable(odometry_converter_node src/odometry_converter_node.cpp) target_link_libraries(odometry_converter_node odometry_converter) cs_install() cs_export() # CMake Indexing FILE(GLOB_RECURSE LibFiles "include/*") add_custom_target(headers SOURCES ${LibFiles}) FILE(GLOB_RECURSE ProtoFiles "proto/*") add_custom_target(protos SOURCES ${ProtoFiles}) ================================================ FILE: odometry_converter/include/odometry_converter/odometry_converter.h ================================================ #pragma once #include #include #include #include #include #include #include "nav_msgs/Odometry.h" #include "quadrotor_msgs/AutopilotFeedback.h" #include "quadrotor_msgs/ControlCommand.h" #include "pose_utils/pose.h" #include "sensor_msgs/Imu.h" #include "std_msgs/Bool.h" #include "std_msgs/Int8.h" namespace odometry_converter { class OdometryConverter { public: OdometryConverter(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); OdometryConverter() : OdometryConverter(ros::NodeHandle(), ros::NodeHandle("~")) {} private: enum STATES { kGroundTruth = 0, kVIO = 1, kChimaera = 2 }; void odometryCallback(const nav_msgs::OdometryConstPtr& msg); void groundTruthCallback(const nav_msgs::OdometryConstPtr& msg); // void switchCallback(const std_msgs::BoolConstPtr& msg); void switchCallback(const std_msgs::Int8ConstPtr& msg); bool loadParameters(); ros::NodeHandle nh_; ros::NodeHandle pnh_; ros::Subscriber odometry_sub_; ros::Subscriber ground_truth_sub_; ros::Subscriber switch_sub_; ros::Publisher odometry_pub_; ros::Publisher odometry_pub_vio_; nav_msgs::Odometry vision_based_odometry_; nav_msgs::Odometry ground_truth_odometry_; rpg::Pose T_W_B_; rpg::Pose T_V_B_; rpg::Pose T_W_V_; rpg::Pose T_B_S_; STATES switch_odometry_ = STATES::kGroundTruth; // bool switch_odometry_ = false; tf::TransformBroadcaster tf_broadcaster_; }; } // namespace odometry_converter ================================================ FILE: odometry_converter/package.xml ================================================ odometry_converter 0.0.0 A package containing utils for simulation Elia Kaufmann GNU GPL catkin catkin_simple roscpp minkindr_conversions nav_msgs quadrotor_msgs pose_utils tf ================================================ FILE: odometry_converter/src/odometry_converter.cpp ================================================ #include "odometry_converter/odometry_converter.h" #include "minkindr_conversions/kindr_msg.h" namespace odometry_converter { OdometryConverter::OdometryConverter(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) : nh_(nh), pnh_(pnh) { if (!loadParameters()) { ROS_ERROR("[%s] Could not load parameters.", pnh_.getNamespace().c_str()); } odometry_sub_ = nh_.subscribe("odometry_in", 1, &OdometryConverter::odometryCallback, this, ros::TransportHints().tcpNoDelay()); ground_truth_sub_ = nh_.subscribe("ground_truth_in", 1, &OdometryConverter::groundTruthCallback, this, ros::TransportHints().tcpNoDelay()); switch_sub_ = nh_.subscribe("switch_odometry", 1, &OdometryConverter::switchCallback, this); odometry_pub_ = nh_.advertise("odometry_out", 0); odometry_pub_vio_ = nh_.advertise( "/hummingbird/odometry_converted_vio", 0); } void OdometryConverter::groundTruthCallback( const nav_msgs::OdometryConstPtr& msg) { double quat_norm = std::sqrt(std::pow(msg->pose.pose.orientation.w, 2) + std::pow(msg->pose.pose.orientation.x, 2) + std::pow(msg->pose.pose.orientation.y, 2) + std::pow(msg->pose.pose.orientation.z, 2)); if (quat_norm < 0.9999 || quat_norm > 1.0001) { ROS_WARN("Invalid orientation of ground truth (norm of quaternion = %.3f)!", quat_norm); } ground_truth_odometry_ = *msg; if (quat_norm > 0.0) { // fix qw to be in the positive half space double multiplier = 1.0; if (msg->pose.pose.orientation.w < 0.0) { multiplier = -1.0; } ground_truth_odometry_.pose.pose.orientation.w = multiplier / quat_norm * msg->pose.pose.orientation.w; ground_truth_odometry_.pose.pose.orientation.x = multiplier / quat_norm * msg->pose.pose.orientation.x; ground_truth_odometry_.pose.pose.orientation.y = multiplier / quat_norm * msg->pose.pose.orientation.y; ground_truth_odometry_.pose.pose.orientation.z = multiplier / quat_norm * msg->pose.pose.orientation.z; } else { ground_truth_odometry_.pose.pose.orientation.w = 1.0; ground_truth_odometry_.pose.pose.orientation.x = 0.0; ground_truth_odometry_.pose.pose.orientation.y = 0.0; ground_truth_odometry_.pose.pose.orientation.z = 0.0; } tf::Transform transform; transform.setOrigin(tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z)); tf::Quaternion q; q.setRPY(0, 0, 0); transform.setRotation(q); tf_broadcaster_.sendTransform(tf::StampedTransform( transform, ros::Time::now(), "world", "/follower_camera_target")); if (switch_odometry_ == STATES::kGroundTruth) { odometry_pub_.publish(ground_truth_odometry_); odometry_pub_vio_.publish(ground_truth_odometry_); } } void OdometryConverter::odometryCallback( const nav_msgs::OdometryConstPtr& msg) { double quat_norm = std::sqrt(std::pow(msg->pose.pose.orientation.w, 2) + std::pow(msg->pose.pose.orientation.x, 2) + std::pow(msg->pose.pose.orientation.y, 2) + std::pow(msg->pose.pose.orientation.z, 2)); if (quat_norm < 0.9999 || quat_norm > 1.0001) { ROS_WARN_THROTTLE(1.0, "Invalid orientation of VIO (norm of quaternion = %.3f)!", quat_norm); } vision_based_odometry_ = *msg; if (quat_norm > 0.0) { vision_based_odometry_.pose.pose.orientation.w = 1.0 / quat_norm * msg->pose.pose.orientation.w; vision_based_odometry_.pose.pose.orientation.x = 1.0 / quat_norm * msg->pose.pose.orientation.x; vision_based_odometry_.pose.pose.orientation.y = 1.0 / quat_norm * msg->pose.pose.orientation.y; vision_based_odometry_.pose.pose.orientation.z = 1.0 / quat_norm * msg->pose.pose.orientation.z; } else { vision_based_odometry_.pose.pose.orientation.w = 1.0; vision_based_odometry_.pose.pose.orientation.x = 0.0; vision_based_odometry_.pose.pose.orientation.y = 0.0; vision_based_odometry_.pose.pose.orientation.z = 0.0; } // VINS Mono publishes linear velocity estimates in world frame, transform // them to body frame: Eigen::Vector3d vlin_world = Eigen::Vector3d(vision_based_odometry_.twist.twist.linear.x, vision_based_odometry_.twist.twist.linear.y, vision_based_odometry_.twist.twist.linear.z); Eigen::Quaterniond q_eigen = Eigen::Quaterniond(vision_based_odometry_.pose.pose.orientation.w, vision_based_odometry_.pose.pose.orientation.x, vision_based_odometry_.pose.pose.orientation.y, vision_based_odometry_.pose.pose.orientation.z); Eigen::Vector3d vlin_body = q_eigen.inverse() * vlin_world; vision_based_odometry_.twist.twist.linear.x = vlin_body.x(); vision_based_odometry_.twist.twist.linear.y = vlin_body.y(); vision_based_odometry_.twist.twist.linear.z = vlin_body.z(); tf::poseMsgToKindr(vision_based_odometry_.pose.pose, &T_V_B_); nav_msgs::Odometry converted_odometry; T_W_B_ = T_W_V_ * T_V_B_; tf::poseKindrToMsg(T_W_B_, &converted_odometry.pose.pose); // fix qw to be in the positive half space if (converted_odometry.pose.pose.orientation.w < 0.0) { converted_odometry.pose.pose.orientation.w = -1.0 * converted_odometry.pose.pose.orientation.w; converted_odometry.pose.pose.orientation.x = -1.0 * converted_odometry.pose.pose.orientation.x; converted_odometry.pose.pose.orientation.y = -1.0 * converted_odometry.pose.pose.orientation.y; converted_odometry.pose.pose.orientation.z = -1.0 * converted_odometry.pose.pose.orientation.z; } if (switch_odometry_ == STATES::kVIO) { converted_odometry.header = ground_truth_odometry_.header; converted_odometry.twist = vision_based_odometry_.twist; odometry_pub_.publish(ground_truth_odometry_); odometry_pub_vio_.publish(converted_odometry); } else if (switch_odometry_ == STATES::kChimaera) { // We take Position&Orientation from the VIO (Position is anyway not used // later...) converted_odometry.header = ground_truth_odometry_.header; // But we take linear&angular velocities from ground truth converted_odometry.twist = ground_truth_odometry_.twist; odometry_pub_.publish(ground_truth_odometry_); odometry_pub_vio_.publish(converted_odometry); } } void OdometryConverter::switchCallback(const std_msgs::Int8ConstPtr& msg) { if (msg->data > 0 && switch_odometry_ == STATES::kGroundTruth) { ROS_INFO("Received switch odometry signal: %d", msg->data); tf::poseMsgToKindr(vision_based_odometry_.pose.pose, &T_V_B_); tf::poseMsgToKindr(ground_truth_odometry_.pose.pose, &T_W_B_); T_W_V_ = T_W_B_ * T_V_B_.inverse(); switch (msg->data) { case STATES::kVIO: { ROS_INFO("Switch to VIO mode!"); switch_odometry_ = STATES::kVIO; break; } case STATES::kChimaera: { ROS_INFO("Switch to Chimaera mode!"); switch_odometry_ = STATES::kChimaera; break; } } } else if (msg->data && switch_odometry_ == true) { // these are the subsequent VIO-reinitializations. Here we have to make sure // that the autopilot does NOT see the discontinuities. Therefore, the // reinitialization state should be transformed to the previous hover state // or the previous trajectory end state. } else { ROS_INFO("Received switch odometry signal: %d", msg->data); ROS_INFO("Switch to Ground Truth mode!"); switch_odometry_ = STATES::kGroundTruth; } } bool OdometryConverter::loadParameters() { bool check = true; // initialize T_B_S_ geometry_msgs::Pose t_B_S; t_B_S.position.x = 0.0; t_B_S.position.y = 0.0; t_B_S.position.z = -0.1; // standard realsense mount (60deg downward looking) t_B_S.orientation.w = 0.1830127; t_B_S.orientation.x = 0.6830127; t_B_S.orientation.y = 0.6830127; t_B_S.orientation.z = 0.1830127; tf::poseMsgToKindr(t_B_S, &T_B_S_); return check; } } // namespace odometry_converter ================================================ FILE: odometry_converter/src/odometry_converter_node.cpp ================================================ #include #include "odometry_converter/odometry_converter.h" int main(int argc, char **argv) { ros::init(argc, argv, "odometry_converter"); odometry_converter::OdometryConverter odometry_converter; ros::spin(); return 0; } ================================================ FILE: pip_requirements.txt ================================================ cachetools==4.0.0 catkin-tools==0.4.5 empy==3.3.4 pyside2==5.13.2 osrf-pycommon==0.1.8 pyqt5-sip==12.7.0 shiboken2==5.13.2 opt-einsum==3.1.0 google-auth==1.11.3 pyasn1-modules==0.2.8 tensorboard==2.1.1 trollius==2.1.post2 pyqt5==5.13.2 catkin-pkg==0.4.14 rospkg==1.1.10 opencv_python==4.2.0 scipy==1.4.1 ================================================ FILE: test_performance/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(test_performance) find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation catkin_simple REQUIRED ) catkin_python_setup() catkin_simple() ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) include_directories( ${catkin_INCLUDE_DIRS} ) cs_export() ================================================ FILE: test_performance/README.md ================================================ # Test Performance Perform multiple experiments to do evaluation/random search of parameters. ================================================ FILE: test_performance/__init__.py ================================================ ================================================ FILE: test_performance/launch/test_performance.launch ================================================ ================================================ FILE: test_performance/nodes/test_performance.py ================================================ #!/usr/bin/env python import rospy from TestPerformance import TestPerformance import os, datetime import numpy as np import random import glob import yaml import sys import time import subprocess # from imutils import paths # import gflags import json import yaml class TestRoutine(object): def __init__(self): full_param_name = rospy.search_param('mpc_dir') mpc_dir = rospy.get_param(full_param_name) mpc_dir = mpc_dir['test_performance']['mpc_dir'] mpc_dir = os.path.join(mpc_dir, 'parameters/default.yaml') self.mpc_params = os.path.abspath(mpc_dir) print(self.mpc_params) self.test_mode = 'mpc' # 'pid' or 'mpc' self.sampling_mode = 'continuous' # 'grid' or 'continuous' self.handtuned_params = True self.params = {} self.idx_array = None self.n_combinations = 1 # will be overwritten in case of grid sampling def init_params(self): print("Initializing parameter distribution") if self.test_mode == 'pid': # PID params self.kdxy = np.arange(20, 0, -2) self.kdz = np.arange(20, 0, -2) self.kpxy = np.arange(20, 0, -2) self.kpz = np.arange(20, 0, -2) self.krp = np.arange(20, 0, -2) self.n_combinations = self.kdxy.size * self.kdz.size * self.kpxy.size * self.kpz.size * self.krp.size self.idx_array = np.zeros((self.n_combinations, 5)) for i in range(self.n_combinations): self.idx_array[i, 0] = i % self.kdxy.size self.idx_array[i, 1] = (i // self.kdxy.size) % self.kdz.size self.idx_array[i, 2] = (i // (self.kdxy.size * self.kdz.size)) % self.kpxy.size self.idx_array[i, 3] = (i // (self.kdxy.size * self.kdz.size * self.kpxy.size)) % self.kpz.size self.idx_array[i, 4] = (i // ( self.kdxy.size * self.kdz.size * self.kpxy.size * self.kpz.size)) % self.krp.size elif self.test_mode == 'mpc': if self.sampling_mode == 'grid': # MPC params self.Q_pos_xy = np.array([0.1, 10.0, 100.0, 1000.0]) self.Q_pos_z = np.array([0.1, 10.0, 100.0, 1000.0]) self.Q_attitude = np.array([0.1, 10.0, 100.0, 1000.0]) self.Q_velocity = np.array([0.1, 10.0, 100.0, 1000.0]) self.R_thrust = np.array([0.1, 1.0, 10.0]) self.R_pitchroll = np.array([0.1, 1.0, 10.0]) self.R_yaw = np.array([0.1, 1.0, 10.0]) self.state_cost_exponential = np.array([0, 0.1, 1.0, 10.0]) self.input_cost_exponential = np.array([0, 0.1, 1.0, 10.0]) self.n_combinations = self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size \ * self.R_thrust.size * self.R_pitchroll.size * self.R_yaw.size \ * self.state_cost_exponential.size * self.input_cost_exponential.size self.idx_array = np.zeros((self.n_combinations, 9)) for i in range(self.n_combinations): self.idx_array[i, 0] = i % self.Q_pos_xy.size self.idx_array[i, 1] = (i // self.Q_pos_xy.size) % self.Q_pos_z.size self.idx_array[i, 2] = (i // (self.Q_pos_xy.size * self.Q_pos_z.size)) % self.Q_attitude.size self.idx_array[i, 3] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size)) % self.Q_velocity.size self.idx_array[i, 4] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size)) % self.R_thrust.size self.idx_array[i, 5] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size * self.R_thrust.size)) % self.R_pitchroll.size self.idx_array[i, 6] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size * self.R_thrust.size * self.R_pitchroll.size)) % self.R_yaw.size self.idx_array[i, 7] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size * self.R_thrust.size * self.R_pitchroll.size * self.R_yaw.size)) % self.state_cost_exponential.size self.idx_array[i, 8] = (i // ( self.Q_pos_xy.size * self.Q_pos_z.size * self.Q_attitude.size * self.Q_velocity.size * self.R_thrust.size * self.R_pitchroll.size * self.R_yaw.size * self.state_cost_exponential.size)) % self.input_cost_exponential.size elif self.sampling_mode == 'continuous': pass def set_params(self, curr_idx): self.params.clear() if self.test_mode == 'pid': pxy_error_max = 0.6 pz_error_max = 0.3 vxy_error_max = 1.0 vz_error_max = 0.75 yaw_error_max = 0.7 self.params['kdxy'] = self.kdxy[int(self.idx_array[curr_idx, 0])] self.params['kdz'] = self.kdz[int(self.idx_array[curr_idx, 1])] self.params['kpxy'] = self.kpxy[int(self.idx_array[curr_idx, 2])] self.params['kpz'] = self.kpz[int(self.idx_array[curr_idx, 3])] self.params['krp'] = self.krp[int(self.idx_array[curr_idx, 4])] self.params['kyaw'] = 5.0 print("Setting control parameters to:") print(self.params) self.params['pxy_error_max'] = pxy_error_max self.params['pz_error_max'] = pz_error_max self.params['vxy_error_max'] = vxy_error_max self.params['vz_error_max'] = vz_error_max self.params['yaw_error_max'] = yaw_error_max os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/k_drag_x 0.0") os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/k_drag_y 0.0") os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/k_drag_z 0.0") os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/k_thrust_horz 0.0") os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/kdxy " + str(self.params['kdxy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/kdz " + str(self.params['kdz'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/kpxy " + str(self.params['kpxy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/kpz " + str(self.params['kpz'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/krp " + str(self.params['krp'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/kyaw " + str(self.params['kyaw'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/position_controller/perform_aerodynamics_compensation false") os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/pxy_error_max " + str( self.params['pxy_error_max'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/pz_error_max " + str( self.params['pz_error_max'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/use_rate_mode true") os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/vxy_error_max " + str( self.params['vxy_error_max'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/vz_error_max " + str( self.params['vz_error_max'])) os.system("timeout 1s rosparam set /hummingbird/autopilot/position_controller/yaw_error_max " + str( self.params['yaw_error_max'])) elif self.test_mode == 'mpc': max_bodyrate_xy = 20.0 max_bodyrate_z = 5.0 min_thrust = 1.0 max_thrust = 40.0 min_val = 0.0001 max_val = 500.0 if self.sampling_mode == 'grid': self.params['Q_pos_xy'] = self.Q_pos_xy[int(self.idx_array[curr_idx, 0])] self.params['Q_pos_z'] = self.Q_pos_z[int(self.idx_array[curr_idx, 1])] self.params['Q_attitude'] = self.Q_attitude[int(self.idx_array[curr_idx, 2])] self.params['Q_velocity'] = self.Q_velocity[int(self.idx_array[curr_idx, 3])] self.params['R_thrust'] = self.R_thrust[int(self.idx_array[curr_idx, 4])] self.params['R_pitchroll'] = self.R_pitchroll[int(self.idx_array[curr_idx, 5])] self.params['R_yaw'] = self.R_yaw[int(self.idx_array[curr_idx, 6])] self.params['state_cost_exponential'] = self.state_cost_exponential[int(self.idx_array[curr_idx, 7])] self.params['input_cost_exponential'] = self.input_cost_exponential[int(self.idx_array[curr_idx, 8])] elif self.sampling_mode == 'continuous': self.params['Q_pos_xy'] = np.random.uniform(low=min_val, high=max_val) self.params['Q_pos_z'] = np.random.uniform(low=min_val, high=max_val) self.params['Q_attitude'] = np.random.uniform(low=min_val, high=max_val) self.params['Q_velocity'] = np.random.uniform(low=min_val, high=max_val) self.params['R_thrust'] = np.random.uniform(low=min_val, high=max_val) self.params['R_pitchroll'] = np.random.uniform(low=min_val, high=max_val) self.params['R_yaw'] = np.random.uniform(low=min_val, high=max_val) self.params['state_cost_exponential'] = np.random.uniform(low=min_val, high=max_val) self.params['input_cost_exponential'] = np.random.uniform(low=min_val, high=max_val) print("Setting control parameters to:") print(self.params) self.params['max_bodyrate_xy'] = max_bodyrate_xy self.params['max_bodyrate_z'] = max_bodyrate_z self.params['min_thrust'] = min_thrust self.params['max_thrust'] = max_thrust os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_pos_xy " + str(self.params['Q_pos_xy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_pos_z " + str(self.params['Q_pos_z'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_attitude " + str(self.params['Q_attitude'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_velocity " + str(self.params['Q_velocity'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_thrust " + str(self.params['R_thrust'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_pitchroll " + str(self.params['R_pitchroll'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_yaw " + str(self.params['R_yaw'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/state_cost_exponential " + str( self.params['state_cost_exponential'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/input_cost_exponential " + str( self.params['input_cost_exponential'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_xy " + str(self.params['max_bodyrate_xy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_z " + str(self.params['max_bodyrate_z'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/min_thrust " + str(self.params['min_thrust'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_thrust " + str(self.params['max_thrust'])) else: assert False, "unknown test mode" def set_handtuned_params(self): self.params.clear() assert self.test_mode == "mpc" max_bodyrate_xy = 30.0 max_bodyrate_z = 10.0 min_thrust = 1.0 max_thrust = 30.0 # read yaml file yaml_path = self.mpc_params with open(yaml_path, 'r') as stream: try: yaml_dict = yaml.safe_load(stream) except yaml.YAMLError as exc: print(exc) self.params['Q_pos_xy'] = yaml_dict['Q_pos_xy'] self.params['Q_pos_z'] = yaml_dict['Q_pos_z'] self.params['Q_attitude'] = yaml_dict['Q_attitude'] self.params['Q_velocity'] = yaml_dict['Q_velocity'] self.params['R_thrust'] = yaml_dict['R_thrust'] self.params['R_pitchroll'] = yaml_dict['R_pitchroll'] self.params['R_yaw'] = yaml_dict['R_yaw'] self.params['state_cost_exponential'] = yaml_dict['state_cost_exponential'] self.params['input_cost_exponential'] = yaml_dict['input_cost_exponential'] self.params['max_bodyrate_xy'] = yaml_dict['max_bodyrate_xy'] self.params['max_bodyrate_z'] = yaml_dict['max_bodyrate_z'] self.params['min_thrust'] = yaml_dict['min_thrust'] self.params['max_thrust'] = yaml_dict['max_thrust'] print("Setting control parameters to:") print(self.params) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_pos_xy " + str(self.params['Q_pos_xy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_pos_z " + str(self.params['Q_pos_z'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_attitude " + str(self.params['Q_attitude'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/Q_velocity " + str(self.params['Q_velocity'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_thrust " + str(self.params['R_thrust'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_pitchroll " + str(self.params['R_pitchroll'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/R_yaw " + str(self.params['R_yaw'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/state_cost_exponential " + str( self.params['state_cost_exponential'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/input_cost_exponential " + str( self.params['input_cost_exponential'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_xy " + str(self.params['max_bodyrate_xy'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_z " + str(self.params['max_bodyrate_z'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/min_thrust " + str(self.params['min_thrust'])) os.system( "timeout 1s rosparam set /hummingbird/autopilot/max_thrust " + str(self.params['max_thrust'])) def run_testing(self): self.init_params() rospy.init_node('test_performance_node', anonymous=True) test_performance = TestPerformance.TestPerformance(self.test_mode) # check if configuration was already tested # data_dir = "/home/elia/Desktop/controller_logs" # files = [f for f in glob.glob(data_dir + "/*.json")] # completed_configs = [f.split('/')[-1] for f in files] if self.handtuned_params: completed_configs = [] #[int(fname[15:-5]) for fname in completed_configs] else: completed_configs = [] #completed_configs = [int(fname[5:-5]) for fname in completed_configs] num_iterations_per_config = 1 for i in range(0, self.n_combinations): j = random.choice(range(0, self.n_combinations)) if j in completed_configs: print("Configuration already tested, continue.") continue completed_configs.append(j) print("Iteration %d of %d, select configuration %d." % (i, self.n_combinations, j)) print("Turn off platform") os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty") if self.handtuned_params: print("Setting HANDTUNED parameters") self.set_handtuned_params() # set parameters read from yaml file else: print("Setting RANDOM parameters") self.set_params(j) # set parameters from random search # reload parameters os.system("timeout 1s rostopic pub /hummingbird/autopilot/reload_parameters std_msgs/Empty") print("Fly trajectory") simulation_alive = test_performance.run(num_iterations_per_config, self.params, j, self.handtuned_params) if not simulation_alive: print("Simulation seems to have crashed, shutting down.") break if __name__ == "__main__": test_routine = TestRoutine() test_routine.run_testing() ================================================ FILE: test_performance/package.xml ================================================ test_performance 0.0.1 Learning to navigate with a realsense tony BSD catkin_simple catkin roscpp rospy std_msgs message_generation ================================================ FILE: test_performance/setup.py ================================================ #!/usr/bin/env python from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['TestPerformance'], package_dir={'': 'src'} ) setup(**d) ================================================ FILE: test_performance/src/TestPerformance/TestPerformance.py ================================================ # /usr/bin/env python import rospy import os import time import json import numpy as np from std_msgs.msg import Empty from quadrotor_msgs.msg import AutopilotFeedback from mav_msgs.msg import Actuators class TestPerformance(object): def __init__(self, test_mode): # os.system("rosservice call /gazebo/unpause_physics") self.finished = False self.airborne = False self.received_heartbeat = False self.test_mode = test_mode self.last_autopilot_state = 0 self.reference_states = [] self.state_estimates = [] self.autopilot_states = [] self.crash_sub = rospy.Subscriber("/hummingbird/command/motor_speed", Actuators, self.callback_motor_speed, queue_size=1) self.autopilot_sub = rospy.Subscriber("/hummingbird/autopilot/feedback", AutopilotFeedback, self.callback_autopilot, queue_size=1) def callback_motor_speed(self, data): self.received_heartbeat = True def callback_autopilot(self, data): if self.airborne: self.reference_states.append([data.reference_state.pose.position.x, data.reference_state.pose.position.y, data.reference_state.pose.position.z, data.reference_state.pose.orientation.w, data.reference_state.pose.orientation.x, data.reference_state.pose.orientation.y, data.reference_state.pose.orientation.z, data.reference_state.velocity.linear.x, data.reference_state.velocity.linear.y, data.reference_state.velocity.linear.z, data.reference_state.velocity.angular.x, data.reference_state.velocity.angular.y, data.reference_state.velocity.angular.z]) self.state_estimates.append([data.state_estimate.pose.pose.position.x, data.state_estimate.pose.pose.position.y, data.state_estimate.pose.pose.position.z, data.state_estimate.pose.pose.orientation.w, data.state_estimate.pose.pose.orientation.x, data.state_estimate.pose.pose.orientation.y, data.state_estimate.pose.pose.orientation.z, data.state_estimate.twist.twist.linear.x, data.state_estimate.twist.twist.linear.y, data.state_estimate.twist.twist.linear.z, data.state_estimate.twist.twist.angular.x, data.state_estimate.twist.twist.angular.y, data.state_estimate.twist.twist.angular.z]) self.autopilot_states.append(data.autopilot_state) # detect crash if data.state_estimate.pose.pose.position.z < 0.1: self.finished = True # Why finished with large X? if data.autopilot_state == 2 and self.last_autopilot_state == 9 and data.state_estimate.pose.pose.position.x > 2.0: print("I am done") self.finished = True self.last_autopilot_state = data.autopilot_state def run(self, num_iterations, params, idx, handtuned_params): # while not rospy.is_shutdown(): # so far, this file is only for testing! self.finished = False self.received_heartbeat = False time_before_crash = [] del self.reference_states[:] del self.state_estimates[:] del self.autopilot_states[:] for i in range(1, num_iterations + 1, 1): self.finished = False print("Replacing quad for new run...") os.system("rosservice call /gazebo/unpause_physics") os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty") # reset quad to initial position os.system( "rosservice call /gazebo/set_model_state '{model_state: { model_name: hummingbird, pose: { position: { x: 0.0, y: 0.0 ,z: 0.2 }, orientation: {x: 0, y: 0, z: 0.0, w: 1.0 } }, twist:{ linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'") # os.system( # "rosservice call /gazebo/set_model_state '{model_state: { model_name: hummingbird, pose: { position: { x: 0.0, y: 0.0 ,z: 0.2 }, orientation: {x: 0, y: 0, z: 0.38268343236, w: 0.92387953251 } }, twist:{ linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'") time.sleep(5) # start quadrotor os.system("timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'") print("Start quadrotor") os.system("timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty") time.sleep(10) self.airborne = True # some initialization motion for VIO os.system("timeout 1s rostopic pub /feature_tracker/restart std_msgs/Bool 'data: true' ") os.system("timeout 1s rostopic pub /hummingbird/autopilot/pose_command geometry_msgs/PoseStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0} , frame_id: world}, pose:{position: { x: 0.0, y: 5.0, z: 2.0}, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0} } }'") time.sleep(10) os.system( "timeout 1s rostopic pub /hummingbird/autopilot/pose_command geometry_msgs/PoseStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0} , frame_id: world}, pose:{position: { x: 0.0, y: 0.0, z: 2.0}, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0} } }'") time.sleep(10) # Switch to vision-based estimate os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Bool 'data: true'") # Network enabled print("Enable flight") os.system("timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Empty") start_time = time.time() while (time.time() - start_time < 30 and self.finished == False): time.sleep(0.5) # x_disturbance = np.sin(time.time() - start_time) # y_disturbance = np.sin(time.time() - start_time) # z_disturbance = np.sin(time.time() - start_time) # duration = 100000000 # duration in nanoseconds # os.system("rosservice call /gazebo/apply_body_wrench " # + "'{body_name: \"hummingbird::hummingbird/base_link\", " # + "wrench: { force: { x: " + str(x_disturbance) # + ", y: " + str(y_disturbance) # + ", z: " + str(z_disturbance) + " } }, start_time: 0, duration: " + str(duration) + " }' ") self.airborne = False time_before_crash.append(time.time() - start_time) print("Experiment finished") # save log to json file result_dict = {'reference_state': self.reference_states, 'state_estimate': self.state_estimates, 'autopilot_states': self.autopilot_states} result_dict.update(params) if handtuned_params: mark_handtuned = "handtuned_" else: mark_handtuned = "" # results_fname = '/home/elia/Desktop/controller_logs/test_' + mark_handtuned + str(idx) + '.json' # print("Saving data to %s" % results_fname) # # print(result_dict) # with open(results_fname, 'w') as outfile: # json.dump(result_dict, outfile) return self.received_heartbeat ================================================ FILE: test_performance/src/TestPerformance/__init__.py ================================================ ================================================ FILE: trajectory_visualizer/.gitignore ================================================ cmake-build-debug/ .idea/ ================================================ FILE: trajectory_visualizer/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(trajectory_visualizer) ## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) add_compile_options(-O3) find_package(catkin_simple REQUIRED) catkin_simple(ALL_DEPS_REQUIRED) cs_add_executable(trajectory_visualizer src/trajectory_visualizer.cpp ) cs_install() cs_export() ================================================ FILE: trajectory_visualizer/include/trajectory_visualizer/trajectory_visualizer.h ================================================ #pragma once #include #include #include namespace trajectory_visualizer { class TrajectoryVisualizer { public: TrajectoryVisualizer(); virtual ~TrajectoryVisualizer(); private: ros::NodeHandle nh_; ros::Publisher marker_pub_gt_; ros::Publisher marker_pub_ref_; ros::Publisher marker_pub_se_; ros::Publisher odometry_ref_pub_; ros::Publisher bodyrates_pub_; ros::Subscriber autopilot_feedback_sub_; ros::Subscriber odometry_gt_sub_; bool loadParameters(); void autopilotFeedbackCallback( const quadrotor_msgs::AutopilotFeedback::ConstPtr& msg); void odometryGTCallback(const nav_msgs::OdometryConstPtr& msg); std::list feedback_queue_; std::list odometry_queue_; // Parameters double n_points_to_visualize_; }; } // namespace trajectory_visualizer ================================================ FILE: trajectory_visualizer/package.xml ================================================ trajectory_visualizer 0.0.0 The trajectory_visualizer package Matthias Faessler TODO Matthias Faessler catkin catkin_simple quadrotor_common quadrotor_msgs roscpp visualization_msgs ================================================ FILE: trajectory_visualizer/src/trajectory_visualizer.cpp ================================================ #include "trajectory_visualizer/trajectory_visualizer.h" #include #include #include namespace trajectory_visualizer { TrajectoryVisualizer::TrajectoryVisualizer() { if (!loadParameters()) { ROS_ERROR("[%s] Failed to load all parameters", ros::this_node::getName().c_str()); ros::shutdown(); } marker_pub_gt_ = nh_.advertise("gt_state", 1); marker_pub_ref_ = nh_.advertise( "autopilot_feedback_trajectory_ref", 1); marker_pub_se_ = nh_.advertise( "autopilot_feedback_trajectory_se", 1); odometry_ref_pub_ = nh_.advertise("reference_odometry", 1); odometry_gt_sub_ = nh_.subscribe("ground_truth/odometry", 1, &TrajectoryVisualizer::odometryGTCallback, this); bodyrates_pub_ = nh_.advertise("bodyrates_viz", 1); autopilot_feedback_sub_ = nh_.subscribe("autopilot/feedback", 1, &TrajectoryVisualizer::autopilotFeedbackCallback, this); } TrajectoryVisualizer::~TrajectoryVisualizer() {} void TrajectoryVisualizer::autopilotFeedbackCallback( const quadrotor_msgs::AutopilotFeedback::ConstPtr& msg) { // store message in buffer feedback_queue_.push_front(*msg); while (feedback_queue_.size() > n_points_to_visualize_) { feedback_queue_.pop_back(); } if (feedback_queue_.size() < 2) { return; } // visualize feedforward bodyrates Eigen::Vector3d bodyrates_bodyframe = Eigen::Vector3d(msg->reference_state.velocity.angular.x, msg->reference_state.velocity.angular.y, msg->reference_state.velocity.angular.z); Eigen::Quaterniond br_bf_q = Eigen::Quaterniond::FromTwoVectors( Eigen::Vector3d::UnitZ(), bodyrates_bodyframe); visualization_msgs::Marker bodyrates_marker; bodyrates_marker.header.frame_id = "hummingbird/base_link"; bodyrates_marker.header.stamp = ros::Time::now(); bodyrates_marker.ns = ""; bodyrates_marker.action = visualization_msgs::Marker::MODIFY; bodyrates_marker.lifetime = ros::Duration(0); bodyrates_marker.type = visualization_msgs::Marker::CUBE; bodyrates_marker.pose.position.x = bodyrates_bodyframe.x() / 2.0; bodyrates_marker.pose.position.y = bodyrates_bodyframe.y() / 2.0; bodyrates_marker.pose.position.z = bodyrates_bodyframe.z() / 2.0; bodyrates_marker.pose.orientation.w = 1.0; bodyrates_marker.pose.orientation.x = 0.0; bodyrates_marker.pose.orientation.y = 0.0; bodyrates_marker.pose.orientation.z = 0.0; bodyrates_marker.id = 0; bodyrates_marker.scale.x = std::abs(bodyrates_bodyframe.x()) + 0.1; bodyrates_marker.scale.y = std::abs(bodyrates_bodyframe.y()) + 0.1; bodyrates_marker.scale.z = std::abs(bodyrates_bodyframe.z()) + 0.1; bodyrates_marker.color.r = 1.0; bodyrates_marker.color.g = 0.0; bodyrates_marker.color.b = 0.0; bodyrates_marker.color.a = 1.0; bodyrates_pub_.publish(bodyrates_marker); // visualize full pose of reference... // combine orientation with heading to single quaternion Eigen::Quaterniond orientation = Eigen::Quaterniond(msg->reference_state.pose.orientation.w, msg->reference_state.pose.orientation.x, msg->reference_state.pose.orientation.y, msg->reference_state.pose.orientation.z); nav_msgs::Odometry reference_odometry; reference_odometry.header.stamp = msg->header.stamp; reference_odometry.header.frame_id = "world"; Eigen::Quaternion q_orientation; Eigen::Quaternion q_heading = Eigen::Quaternion(Eigen::AngleAxis( msg->reference_state.heading, Eigen::Matrix::UnitZ())); q_orientation = q_heading * orientation; reference_odometry.pose.pose.orientation.w = q_orientation.w(); reference_odometry.pose.pose.orientation.x = q_orientation.x(); reference_odometry.pose.pose.orientation.y = q_orientation.y(); reference_odometry.pose.pose.orientation.z = q_orientation.z(); // reference_odometry.pose.pose.orientation = // msg->reference_state.pose.orientation; reference_odometry.pose.pose.position = msg->reference_state.pose.position; odometry_ref_pub_.publish(reference_odometry); /////////////// visualization_msgs::MarkerArray marker_msg_ref; visualization_msgs::MarkerArray marker_msg_se; // General marker visualization_msgs::Marker marker; marker.header.frame_id = "world"; marker.header.stamp = ros::Time::now(); marker.ns = ""; marker.action = visualization_msgs::Marker::MODIFY; marker.lifetime = ros::Duration(0); marker.type = visualization_msgs::Marker::LINE_STRIP; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; marker.pose.position.z = 0.0; marker.pose.orientation.w = 1.0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; // Reference trajectory marker.id = 0; marker.scale.x = 0.015; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0; for (auto it = feedback_queue_.begin(); it != feedback_queue_.end(); it++) { geometry_msgs::Point point; point.x = it->reference_state.pose.position.x; point.y = it->reference_state.pose.position.y; point.z = it->reference_state.pose.position.z; marker.points.push_back(point); } marker_msg_ref.markers.push_back(marker); marker_pub_ref_.publish(marker_msg_ref); // Estimated trajectory marker.id = 1; marker.scale.x = 0.025; marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 1.0; marker.points.clear(); for (auto it = feedback_queue_.begin(); it != feedback_queue_.end(); it++) { geometry_msgs::Point point; point.x = it->state_estimate.pose.pose.position.x; point.y = it->state_estimate.pose.pose.position.y; point.z = it->state_estimate.pose.pose.position.z; marker.points.push_back(point); } marker_msg_se.markers.push_back(marker); marker_pub_se_.publish(marker_msg_se); } void TrajectoryVisualizer::odometryGTCallback( const nav_msgs::OdometryConstPtr& msg) { // store message in buffer odometry_queue_.push_front(*msg); while (odometry_queue_.size() > 20 * n_points_to_visualize_) { odometry_queue_.pop_back(); } if (odometry_queue_.size() < 2) { return; } /////////////// visualization_msgs::MarkerArray marker_msg_gt; // General marker visualization_msgs::Marker marker; marker.header.frame_id = "world"; marker.header.stamp = ros::Time::now(); marker.ns = ""; marker.action = visualization_msgs::Marker::MODIFY; marker.lifetime = ros::Duration(0); marker.type = visualization_msgs::Marker::LINE_STRIP; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; marker.pose.position.z = 0.0; marker.pose.orientation.w = 1.0; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; // Reference trajectory marker.id = 0; marker.scale.x = 0.025; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0; for (auto it = odometry_queue_.begin(); it != odometry_queue_.end(); it++) { geometry_msgs::Point point; point.x = it->pose.pose.position.x; point.y = it->pose.pose.position.y; point.z = it->pose.pose.position.z; marker.points.push_back(point); } marker_msg_gt.markers.push_back(marker); marker_pub_gt_.publish(marker_msg_gt); } bool TrajectoryVisualizer::loadParameters() { if (!quadrotor_common::getParam("n_points_to_visualize", n_points_to_visualize_, 50.0)) { return false; } return true; } } // namespace trajectory_visualizer int main(int argc, char** argv) { ros::init(argc, argv, "trajectory_visualizer"); trajectory_visualizer::TrajectoryVisualizer trajectory_visualizer; ros::spin(); return 0; }