[ { "path": ".clang-format", "content": "---\nLanguage: Cpp\n# BasedOnStyle: Google\nAccessModifierOffset: -1\nAlignAfterOpenBracket: Align\nAlignConsecutiveAssignments: false\nAlignConsecutiveDeclarations: false\nAlignEscapedNewlines: Left\nAlignOperands: true\nAlignTrailingComments: true\nAllowAllParametersOfDeclarationOnNextLine: true\nAllowShortBlocksOnASingleLine: false\nAllowShortCaseLabelsOnASingleLine: false\nAllowShortFunctionsOnASingleLine: All\nAllowShortIfStatementsOnASingleLine: true\nAllowShortLoopsOnASingleLine: true\nAlwaysBreakAfterDefinitionReturnType: None\nAlwaysBreakAfterReturnType: None\nAlwaysBreakBeforeMultilineStrings: true\nAlwaysBreakTemplateDeclarations: true\nBinPackArguments: true\nBinPackParameters: true\nBraceWrapping: \n AfterClass: false\n AfterControlStatement: false\n AfterEnum: false\n AfterFunction: false\n AfterNamespace: false\n AfterObjCDeclaration: false\n AfterStruct: false\n AfterUnion: false\n AfterExternBlock: false\n BeforeCatch: false\n BeforeElse: false\n IndentBraces: false\n SplitEmptyFunction: true\n SplitEmptyRecord: true\n SplitEmptyNamespace: true\nBreakBeforeBinaryOperators: None\nBreakBeforeBraces: Attach\nBreakBeforeInheritanceComma: false\nBreakBeforeTernaryOperators: true\nBreakConstructorInitializersBeforeComma: false\nBreakConstructorInitializers: BeforeColon\nBreakAfterJavaFieldAnnotations: false\nBreakStringLiterals: true\nColumnLimit: 80\nCommentPragmas: '^ IWYU pragma:'\nCompactNamespaces: false\nConstructorInitializerAllOnOneLineOrOnePerLine: true\nConstructorInitializerIndentWidth: 4\nContinuationIndentWidth: 4\nCpp11BracedListStyle: true\nDerivePointerAlignment: true\nDisableFormat: false\nExperimentalAutoDetectBinPacking: false\nFixNamespaceComments: true\nForEachMacros: \n - foreach\n - Q_FOREACH\n - BOOST_FOREACH\nIncludeBlocks: Preserve\nIncludeCategories: \n - Regex: '^'\n Priority: 2\n - Regex: '^<.*\\.h>'\n Priority: 1\n - Regex: '^<.*'\n Priority: 2\n - Regex: '.*'\n Priority: 3\nIncludeIsMainRegex: '([-_](test|unittest))?$'\nIndentCaseLabels: true\nIndentPPDirectives: None\nIndentWidth: 2\nIndentWrappedFunctionNames: false\nJavaScriptQuotes: Leave\nJavaScriptWrapImports: true\nKeepEmptyLinesAtTheStartOfBlocks: false\nMacroBlockBegin: ''\nMacroBlockEnd: ''\nMaxEmptyLinesToKeep: 1\nNamespaceIndentation: None\nObjCBlockIndentWidth: 2\nObjCSpaceAfterProperty: false\nObjCSpaceBeforeProtocolList: false\nPenaltyBreakAssignment: 2\nPenaltyBreakBeforeFirstCallParameter: 1\nPenaltyBreakComment: 300\nPenaltyBreakFirstLessLess: 120\nPenaltyBreakString: 1000\nPenaltyExcessCharacter: 1000000\nPenaltyReturnTypeOnItsOwnLine: 200\nPointerAlignment: Left\n#RawStringFormats:\n# - Delimiter: pb\n# Language: TextProto\n# BasedOnStyle: google\nReflowComments: true\nSortIncludes: true\nSortUsingDeclarations: true\nSpaceAfterCStyleCast: false\nSpaceAfterTemplateKeyword: true\nSpaceBeforeAssignmentOperators: true\nSpaceBeforeParens: ControlStatements\nSpaceInEmptyParentheses: false\nSpacesBeforeTrailingComments: 2\nSpacesInAngles: false\nSpacesInContainerLiterals: true\nSpacesInCStyleCastParentheses: false\nSpacesInParentheses: false\nSpacesInSquareBrackets: false\nStandard: Auto\nTabWidth: 8\nUseTab: Never\n...\n\n" }, { "path": ".gitignore", "content": ".idea/\ncmake-build-debug/\n__pycache__/\n*.pyc\n" }, { "path": "LICENSE", "content": "MIT License\n\nCopyright (c) 2020 Robotics and Perception Group\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n" }, { "path": "README.md", "content": "# Deep Drone Acrobatics\n\nThis repo contains the code associated to our paper Deep Drone Acrobatics. \n\n

\n \"dda\"\n

\n\n\n#### Citing\n\nIf you use this code in an academic context, please cite the following publication:\n\nPaper: [Deep Drone Acrobatics](http://rpg.ifi.uzh.ch/docs/RSS20_Kaufmann.pdf)\n\nRSS 2020 Video Pitch: [Youtube](https://youtu.be/r4zzdFw87CY)\n\nVideo: [YouTube](https://youtu.be/2N_wKXQ6MXA)\n\n```\n@inproceedings{kaufmann2020RSS,\n title={Deep Drone Acrobatics},\n author={Kaufmann, Elia and Loquercio, Antonio and Ranftl, Ren{\\'e} and M{\\\"u}ller, Matthias and Koltun, Vladlen and Scaramuzza, Davide},\n booktitle={Proceedings of Robotics: Science and Systems}, \n year={2020}, \n address={Corvalis, Oregon, USA}, \n month={July}, \n doi={10.15607/RSS.2020.XVI.040} \n} \n```\n\n## Installation\n\n### Requirements\n\nThe code was tested with Ubuntu 18.04, ROS Melodic, Anaconda v4.8.3.\nDifferent OS and ROS versions are possible but not supported.\n\n\n### Step-by-Step Procedure\n\nUse the following commands to create a new catkin workspace and a virtual environment with all the required dependencies.\n\n```bash\nexport ROS_VERSION=melodic\nmkdir drone_acrobatics_ws\ncd drone_acrobatics_ws\nexport CATKIN_WS=./catkin_dda\nmkdir -p $CATKIN_WS/src\ncd $CATKIN_WS\ncatkin init\ncatkin config --extend /opt/ros/$ROS_VERSION\ncatkin config --merge-devel\ncatkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-fdiagnostics-color\ncd src\n\ngit clone https://github.com/uzh-rpg/deep_drone_acrobatics.git\nvcs-import < deep_drone_acrobatics/dependencies.yaml\n\n#install extra dependencies (might need more depending on your OS)\nsudo apt-get install libqglviewer-dev-qt5\n```\n\nBefore continuing, make sure that your protobuf compiler version is 3.0.0.\nTo check this out, type in a terminal ``protoc --version``.\nIf 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.\n\nNote 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).\n\n```bash\n# Build and re-source the workspace\ncatkin build\n. ../devel/setup.bash\n\n# Create your learning environment\ncd deep_drone_acrobatics\nconda create --name drone_flow python=3.6\nconda activate drone_flow\n# Install (in an hacky way) python requirements\npip install -r pip_requirements.txt\nconda install --file conda_requirements.txt\n\n```\n\n\n## Let's do a Power Loop\n\nOnce 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.\n\nLauch a simulation! Open a terminal and type:\n```bash\ncd drone_acrobatics_ws\nsource catkin_dda/devel/setup.bash\nroslaunch fpv_aggressive_trajectories simulation.launch\n```\n\nRun the Network in an other terminal:\n```bash\ncd\ncd drone_acrobatics_ws\n. ./catkin_dda/devel/setup.bash\nconda activate drone_flow\npython test_trajectories.py --settings_file=config/test_settings.yaml\n\n```\n\n## Train your own acrobatic controller\n\nYou 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!).\n\n### Generate data\n\nLaunch the simulation in one terminal\n```bash\ncd drone_acrobatics_ws\nsource catkin_dda/devel/setup.bash\nroslaunch fpv_aggressive_trajectories simulation.launch\n```\n\nLaunch data collection (with dagger) in an other terminal\n```bash\ncd\ncd drone_acrobatics_ws\n. ./catkin_dda/devel/setup.bash\nconda activate drone_flow\npython iterative_learning_trajectories.py --settings_file=config/dagger_settings.yaml\n```\n\nIt 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.\n\n\n### Train the Network\n\nIf you want to train the network on data already collected (for example to do some parameter tuning) use the following commands.\nMake sure to adapt the settings file to your configuration.\n\n```bash\ncd\ncd drone_acrobatics_ws\n. ./catkin_dda/devel/setup.bash\nconda activate drone_flow\npython train.py --settings_file=config/train_settings.yaml\n```\n\n### Test the Network\n\nTo 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!\n\nLauch a simulation! Open a terminal and type:\n```bash\ncd drone_acrobatics_ws\nsource catkin_dda/devel/setup.bash\nroslaunch fpv_aggressive_trajectories simulation.launch\n```\n\nRun the Network in an other terminal:\n```bash\ncd\ncd drone_acrobatics_ws\n. ./catkin_dda/devel/setup.bash\nconda activate drone_flow\npython test_trajectories.py --settings_file=config/test_settings.yaml\n\n```\n" }, { "path": "acrobatic_trajectory_helper/.gitignore", "content": ".idea/\ncmake-build-debug/\n" }, { "path": "acrobatic_trajectory_helper/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(acrobatic_trajectory_helper)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-std=c++11)\nadd_compile_options(-O3)\n\nfind_package(catkin_simple REQUIRED)\ncatkin_simple(ALL_DEPS_REQUIRED)\n\ncs_add_library(${PROJECT_NAME} src/polynomial_trajectory_helper.cpp\n\tsrc/heading_trajectory_helper.cpp src/circle_trajectory_helper.cpp)\n\ncs_add_library(acrobatic_sequence src/acrobatic_sequence)\ntarget_link_libraries(acrobatic_sequence ${PROJECT_NAME})\n\ncs_install()\ncs_export()\n" }, { "path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/acrobatic_sequence.h", "content": "#pragma once\n\n#include \n#include \n\nnamespace fpv_aggressive_trajectories {\nclass AcrobaticSequence {\n public:\n explicit AcrobaticSequence(\n const quadrotor_common::TrajectoryPoint& start_state);\n\n virtual ~AcrobaticSequence();\n\n bool appendLoops(const int n_loops, const double& circle_velocity,\n const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end,\n const bool break_at_end, const double& traj_sampling_freq);\n\n bool appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end,\n const bool break_at_end);\n\n bool appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end);\n\n std::list getManeuverList();\n\n private:\n std::list maneuver_list_;\n};\n} // namespace fpv_aggressive_trajectories\n" }, { "path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/circle_trajectory_helper.h", "content": "#pragma once\n\n#include \n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace circles {\n\nquadrotor_common::Trajectory computeHorizontalCircleTrajectory(\n const Eigen::Vector3d center, const double radius, const double speed,\n const double phi_start, const double phi_end,\n const double sampling_frequency);\n\nquadrotor_common::Trajectory computeVerticalCircleTrajectory(\n const Eigen::Vector3d center, const double orientation, const double radius,\n const double speed, const double phi_start, const double phi_end,\n const double sampling_frequency);\n\nquadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew(\n const Eigen::Vector3d center, const double orientation, const double radius,\n const double speed, const double phi_start, const double phi_end,\n const double corkscrew_velocity, const double sampling_frequency);\n\n} // namespace circles\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/heading_trajectory_helper.h", "content": "#pragma once\n\n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace heading {\n\nvoid addConstantHeading(const double heading,\n quadrotor_common::Trajectory* trajectory);\n\nvoid addConstantHeadingRate(const double initial_heading,\n const double final_heading,\n quadrotor_common::Trajectory* trajectory);\n\n} // namespace heading\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/polynomial_trajectory_helper.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace polynomials {\n\n// Constrained Polynomials\nquadrotor_common::Trajectory computeTimeOptimalTrajectory(\n const quadrotor_common::TrajectoryPoint& s0,\n const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency);\n\nquadrotor_common::Trajectory computeFixedTimeTrajectory(\n const quadrotor_common::TrajectoryPoint& s0,\n const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,\n const double execution_time, const double sampling_frequency);\n\n// Minimum Snap Style Polynomials\nquadrotor_common::Trajectory generateMinimumSnapTrajectory(\n const Eigen::VectorXd& segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency);\nquadrotor_common::Trajectory generateMinimumSnapTrajectory(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency);\n\nquadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency);\nquadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency);\n\nquadrotor_common::Trajectory generateMinimumSnapRingTrajectory(\n const Eigen::VectorXd& segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency);\nquadrotor_common::Trajectory generateMinimumSnapRingTrajectory(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency);\n\nquadrotor_common::Trajectory\ngenerateMinimumSnapRingTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency);\nquadrotor_common::Trajectory\ngenerateMinimumSnapRingTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency);\n\n// Sampling function\nquadrotor_common::Trajectory samplePolynomial(\n const polynomial_trajectories::PolynomialTrajectory& polynomial,\n const double sampling_frequency);\n\n} // namespace polynomials\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "acrobatic_trajectory_helper/package.xml", "content": "\n\n acrobatic_trajectory_helper\n 0.0.0\n The acrobatic_trajectory_helper package\n\n Elia Kaufmann\n MIT\n\n Elia Kaufmann\n\n catkin\n catkin_simple\n\n eigen_catkin\n minimum_jerk_trajectories\n polynomial_trajectories\n quadrotor_common\n roscpp\n\n \n\n \n\n\n" }, { "path": "acrobatic_trajectory_helper/src/acrobatic_sequence.cpp", "content": "#include \"acrobatic_trajectory_helper/acrobatic_sequence.h\"\n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\nnamespace fpv_aggressive_trajectories {\nAcrobaticSequence::AcrobaticSequence(\n const quadrotor_common::TrajectoryPoint& start_state) {\n printf(\"Initiated acrobatic sequence\\n\");\n quadrotor_common::Trajectory init_trajectory;\n quadrotor_common::TrajectoryPoint init_point;\n init_point = start_state;\n init_trajectory.points.push_back(init_point);\n maneuver_list_.push_back(init_trajectory);\n}\n\nAcrobaticSequence::~AcrobaticSequence() {}\n\nbool AcrobaticSequence::appendLoops(\n const int n_loops, const double& circle_velocity, const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end,\n const double& traj_sampling_freq) {\n printf(\"appending loop\\n\");\n\n // get start state\n quadrotor_common::TrajectoryPoint init_state =\n maneuver_list_.back().points.back();\n\n printf(\n \"Enter state of loop maneuver: Pos: %.2f, %.2f, %.2f | Vel: %.2f, %.2f, \"\n \"%.2f\\n\",\n init_state.position.x(), init_state.position.y(), init_state.position.z(),\n init_state.velocity.x(), init_state.velocity.y(),\n init_state.velocity.z());\n\n const double exec_loop_rate = traj_sampling_freq;\n const double desired_heading = 0.0;\n\n const double figure_z_rotation_angle = 0.0;\n // const double figure_z_rotation_angle = 0.785398163;\n\n const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(\n Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));\n double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(\n desired_heading + figure_z_rotation_angle);\n\n // cirlce center RELATIVE to start position\n const Eigen::Vector3d circle_center =\n init_state.position + q_W_P.inverse() * circle_center_offset;\n\n printf(\"circle center: %.2f, %.2f, %.2f\\n\", circle_center.x(),\n circle_center.y(), circle_center.z());\n\n const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius;\n const double max_roll_pitch_rate = 3.0;\n\n // Compute Circle trajectory\n printf(\"compute circle trajectory\\n\");\n\n quadrotor_common::Trajectory circle_trajectory =\n acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory(\n circle_center, figure_z_rotation_angle, radius, circle_velocity,\n M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n desired_heading_loop, &circle_trajectory);\n\n quadrotor_common::TrajectoryPoint circle_enter_state =\n circle_trajectory.points.front();\n\n // Start position relative to circle center\n quadrotor_common::TrajectoryPoint start_state;\n start_state = init_state;\n\n // enter trajectory\n printf(\"compute enter trajectory\\n\");\n // printf(\"Maximum speed: %.3f, current speed: %.3f\\n\", 1.1*circle_velocity,\n // start_state.velocity.norm());\n quadrotor_common::Trajectory enter_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n start_state, circle_enter_state, 4,\n 1.1 * std::max(start_state.velocity.norm(), circle_velocity),\n max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n desired_heading_loop, &enter_trajectory);\n\n // End position RELATIVE to circle center\n printf(\"compute exit trajectory\\n\");\n\n const Eigen::Vector3d end_pos_P =\n circle_center_offset_end; // Eigen::Vector3d(circle_center_offset.x(),\n // scircle_center_offset.y(),-circle_center_offset.z()); // nice breaking\n // forward flip\n quadrotor_common::TrajectoryPoint end_state;\n end_state.position = q_W_P * end_pos_P + circle_center;\n end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0);\n\n quadrotor_common::Trajectory exit_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust,\n 2.0 * max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n desired_heading_loop, &exit_trajectory);\n\n quadrotor_common::Trajectory breaking_trajectory;\n breaking_trajectory.trajectory_type =\n quadrotor_common::Trajectory::TrajectoryType::GENERAL;\n\n maneuver_list_.push_back(enter_trajectory);\n maneuver_list_.push_back(circle_trajectory);\n maneuver_list_.push_back(exit_trajectory);\n\n if (break_at_end) {\n // append breaking trajectory at end\n quadrotor_common::TrajectoryPoint end_state_hover;\n end_state_hover.position =\n (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));\n end_state_hover.velocity = Eigen::Vector3d::Zero();\n breaking_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,\n max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n 0.0, &breaking_trajectory);\n maneuver_list_.push_back(breaking_trajectory);\n }\n\n return !(enter_trajectory.trajectory_type ==\n quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||\n circle_trajectory.trajectory_type ==\n quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||\n exit_trajectory.trajectory_type ==\n quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||\n breaking_trajectory.trajectory_type ==\n quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);\n}\n\nbool AcrobaticSequence::appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end) {\n printf(\"appending matty loop\\n\");\n\n // get start state\n quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back();\n\n const double exec_loop_rate = 50.0;\n const double desired_heading = M_PI;\n\n const double figure_z_rotation_angle = 0.0;\n\n const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(\n Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));\n double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(\n desired_heading + figure_z_rotation_angle);\n\n // cirlce center RELATIVE to start position\n const Eigen::Vector3d circle_center = init_state.position + q_W_P.inverse() * circle_center_offset;\n\n const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius;\n const double max_roll_pitch_rate = 3.0;\n\n quadrotor_common::Trajectory circle_trajectory =\n acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory(\n circle_center, figure_z_rotation_angle, radius,\n circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n desired_heading_loop, &circle_trajectory);\n\n quadrotor_common::TrajectoryPoint circle_enter_state =\n circle_trajectory.points.front();\n\n // Start position relative to circle center\n quadrotor_common::TrajectoryPoint start_state;\n start_state = init_state;\n\n // enter trajectory\n quadrotor_common::Trajectory enter_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n start_state, circle_enter_state, 4, 1.1 * std::max(start_state.velocity.norm(), circle_velocity),\n max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);\n\n acrobatic_trajectory_helper::heading::addConstantHeadingRate(0.0,\n M_PI,\n &enter_trajectory);\n\n const Eigen::Vector3d end_pos_P = circle_center_offset_end; \n quadrotor_common::TrajectoryPoint end_state;\n end_state.position = q_W_P * end_pos_P + circle_center;\n end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0);\n\n quadrotor_common::Trajectory exit_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust,\n 2.0 * max_roll_pitch_rate, exec_loop_rate);\n\n acrobatic_trajectory_helper::heading::addConstantHeadingRate(M_PI,\n 2 * M_PI,\n &exit_trajectory);\n\n // append breaking trajectory at end\n quadrotor_common::TrajectoryPoint end_state_hover;\n end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));\n end_state_hover.velocity = Eigen::Vector3d::Zero();\n quadrotor_common::Trajectory breaking_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,\n max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n 0.0,\n &breaking_trajectory);\n\n maneuver_list_.push_back(enter_trajectory);\n maneuver_list_.push_back(circle_trajectory);\n maneuver_list_.push_back(exit_trajectory);\n maneuver_list_.push_back(breaking_trajectory);\n\n return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);\n}\n \n \nbool AcrobaticSequence::appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius,\n const Eigen::Vector3d& circle_center_offset,\n const Eigen::Vector3d& circle_center_offset_end,\n const bool break_at_end) {\n printf(\"appending barell roll\\n\");\n\n // get start state\n quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back();\n\n const double exec_loop_rate = 50.0;\n const double desired_heading = 0.0;\n const double circle_radius = radius;\n double corkscrew_velocity = 0.5;\n const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / circle_radius;\n const double max_roll_pitch_rate = 3.0;\n const double figure_z_rotation_angle = M_PI / 2.0;\n double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(\n desired_heading + figure_z_rotation_angle);\n\n quadrotor_common::TrajectoryPoint circle_enter_state;\n circle_enter_state.position = init_state.position + circle_center_offset;\n circle_enter_state.velocity = Eigen::Vector3d(corkscrew_velocity, circle_velocity, 0.0);\n\n // compute enter trajectory\n quadrotor_common::Trajectory enter_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n init_state, circle_enter_state, 4, 1.5 * std::max(init_state.velocity.norm(), circle_velocity),\n max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(0.0,\n &enter_trajectory);\n // cirlce center RELATIVE to start position\n const Eigen::Vector3d circle_center = circle_enter_state.position + Eigen::Vector3d(0.0, 0.0, circle_radius);\n\n // Compute Circle trajectory\n double rotate_loop = M_PI / 2.0;\n quadrotor_common::Trajectory circle_trajectory =\n acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectoryCorkScrew(\n circle_center, rotate_loop, circle_radius,\n circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, corkscrew_velocity, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n 0, &circle_trajectory);\n\n const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(\n Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));\n\n quadrotor_common::TrajectoryPoint end_state;\n end_state.position = circle_trajectory.points.back().position + circle_center_offset_end;\n end_state.velocity = Eigen::Vector3d(circle_velocity, 0.0, 0.0);\n\n quadrotor_common::TrajectoryPoint circle_exit_state = circle_trajectory.points.back();\n\n quadrotor_common::Trajectory exit_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n circle_exit_state, end_state, 4, 1.5 * circle_velocity, max_thrust,\n 2.0 * max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(0.0,\n &exit_trajectory);\n\n maneuver_list_.push_back(enter_trajectory);\n maneuver_list_.push_back(circle_trajectory);\n maneuver_list_.push_back(exit_trajectory);\n\n quadrotor_common::Trajectory breaking_trajectory;\n breaking_trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL;\n if (break_at_end) {\n // append breaking trajectory at end\n quadrotor_common::TrajectoryPoint end_state_hover;\n end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));\n end_state_hover.velocity = Eigen::Vector3d::Zero();\n breaking_trajectory =\n acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(\n end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,\n max_roll_pitch_rate, exec_loop_rate);\n acrobatic_trajectory_helper::heading::addConstantHeading(\n 0.0,\n &breaking_trajectory);\n\n maneuver_list_.push_back(breaking_trajectory);\n }\n\n // Debug output\n std::cout << static_cast(enter_trajectory.trajectory_type) << std::endl;\n std::cout << static_cast(circle_trajectory.trajectory_type) << std::endl;\n std::cout << static_cast(exit_trajectory.trajectory_type) << std::endl;\n\n return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED\n || breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);\n}\n \nstd::list AcrobaticSequence::getManeuverList() {\n return maneuver_list_;\n}\n\n} // namespace fpv_aggressive_trajectories\n" }, { "path": "acrobatic_trajectory_helper/src/circle_trajectory_helper.cpp", "content": "#include \"acrobatic_trajectory_helper/circle_trajectory_helper.h\"\n\n#include \n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace circles {\n\nquadrotor_common::Trajectory computeVerticalCircleTrajectory(\n const Eigen::Vector3d center, const double orientation, const double radius,\n const double speed, const double phi_start, const double phi_end,\n const double sampling_frequency) {\n /*\n * We use a coordinate system with x to the front, y to the left, and z up.\n * Orientation is the angle by which the circle is rotated about the z-axis\n * If the orientation = 0.0 then the circle is in the x-z plane\n * When setting phi_start = 0.0 and orientation = 0.0, the start point will\n * be at (radius, 0, 0) + center\n * If phi_end > phi_start the circle is going in positive direction around\n * the y-axis rotated by orientation. Otherwise it is going in negative\n * direction\n */\n\n quadrotor_common::Trajectory trajectory;\n trajectory.trajectory_type =\n quadrotor_common::Trajectory::TrajectoryType::GENERAL;\n\n const double phi_total = phi_end - phi_start;\n const double direction = phi_total / fabs(phi_total);\n const double omega = direction * fabs(speed / radius);\n const double angle_step = fabs(omega / sampling_frequency);\n\n const Eigen::Quaterniond q_ori = Eigen::Quaterniond(\n Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation),\n Eigen::Vector3d::UnitZ()));\n\n for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) {\n const double phi = phi_start + direction * d_phi;\n const double cos_phi = cos(phi);\n const double sin_phi = sin(phi);\n quadrotor_common::TrajectoryPoint point;\n point.time_from_start = ros::Duration(fabs(d_phi / omega));\n point.position =\n q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;\n point.velocity =\n q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));\n point.acceleration = q_ori * (radius * pow(omega, 2.0) *\n Eigen::Vector3d(-cos_phi, 0.0, sin_phi));\n point.jerk = q_ori * (radius * pow(omega, 3.0) *\n Eigen::Vector3d(sin_phi, 0.0, cos_phi));\n point.snap = q_ori * (radius * pow(omega, 4.0) *\n Eigen::Vector3d(cos_phi, 0.0, -sin_phi));\n Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);\n Eigen::Quaterniond quatDes =\n Eigen::Quaterniond::FromTwoVectors(\n I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) *\n Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0,\n std::sin(0.5 * orientation));\n point.orientation = quatDes;\n\n // TODO: compute feedforward bodyrates\n double bodyrate_y = -omega;\n point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0);\n\n trajectory.points.push_back(point);\n }\n\n // Add last point at phi_end\n const double phi = phi_start + phi_total;\n const double cos_phi = cos(phi);\n const double sin_phi = sin(phi);\n quadrotor_common::TrajectoryPoint point;\n point.time_from_start = ros::Duration(fabs(phi_total / omega));\n point.position =\n q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;\n point.velocity =\n q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));\n point.acceleration = q_ori * (radius * pow(omega, 2.0) *\n Eigen::Vector3d(-cos_phi, 0.0, sin_phi));\n point.jerk = q_ori * (radius * pow(omega, 3.0) *\n Eigen::Vector3d(sin_phi, 0.0, cos_phi));\n point.snap = q_ori * (radius * pow(omega, 4.0) *\n Eigen::Vector3d(cos_phi, 0.0, -sin_phi));\n Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);\n Eigen::Quaterniond quatDes =\n Eigen::Quaterniond::FromTwoVectors(\n I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) *\n Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0,\n std::sin(0.5 * orientation));\n point.orientation = quatDes;\n\n trajectory.points.push_back(point);\n\n return trajectory;\n}\n\n\nquadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew(\n const Eigen::Vector3d center, const double orientation, const double radius,\n const double speed, const double phi_start, const double phi_end, const double corkscrew_velocity,\n const double sampling_frequency)\n{\n /*\n * We use a coordinate system with x to the front, y to the left, and z up.\n * Orientation is the angle by which the circle is rotated about the z-axis\n * If the orientation = 0.0 then the circle is in the x-z plane\n * When setting phi_start = 0.0 and orientation = 0.0, the start point will\n * be at (radius, 0, 0) + center\n * If phi_end > phi_start the circle is going in positive direction around\n * the y-axis rotated by orientation. Otherwise it is going in negative\n * direction\n */\n\n quadrotor_common::Trajectory trajectory;\n trajectory.trajectory_type =\n quadrotor_common::Trajectory::TrajectoryType::GENERAL;\n\n const double phi_total = phi_end - phi_start;\n const double direction = phi_total / fabs(phi_total);\n const double omega = direction * fabs(speed / radius);\n const double angle_step = fabs(omega / sampling_frequency);\n const double linear_step = fabs(corkscrew_velocity / sampling_frequency);\n double y_pos = 0.0;\n\n const Eigen::Quaterniond q_ori = Eigen::Quaterniond(\n Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation),\n Eigen::Vector3d::UnitZ()));\n\n for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step)\n {\n const double phi = phi_start + direction * d_phi;\n const double cos_phi = cos(phi);\n const double sin_phi = sin(phi);\n quadrotor_common::TrajectoryPoint point;\n point.time_from_start = ros::Duration(fabs(d_phi / omega));\n point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi))\n + center;\n point.velocity = q_ori\n * (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi));\n point.acceleration = q_ori\n * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi));\n point.jerk = q_ori\n * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi));\n point.snap = q_ori\n * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi));\n Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);\n Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81))\n * Eigen::Quaterniond(std::cos(0.5 * orientation),\n 0.0,\n 0.0,\n std::sin(0.5 * orientation));\n point.orientation = quatDes;\n\n // TODO: compute feedforward bodyrates\n double bodyrate_y = -omega;\n point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0);\n y_pos -= linear_step;\n trajectory.points.push_back(point);\n }\n\n // Add last point at phi_end\n const double phi = phi_start + phi_total;\n const double cos_phi = cos(phi);\n const double sin_phi = sin(phi);\n quadrotor_common::TrajectoryPoint point;\n point.time_from_start = ros::Duration(fabs(phi_total / omega));\n point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi))\n + center;\n point.velocity = q_ori\n * (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi));\n point.acceleration = q_ori\n * (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi));\n point.jerk = q_ori\n * (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi));\n point.snap = q_ori\n * (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi));\n Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);\n Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81))\n * Eigen::Quaterniond(std::cos(0.5 * orientation),\n 0.0,\n 0.0,\n std::sin(0.5 * orientation));\n point.orientation = quatDes;\n\n trajectory.points.push_back(point);\n\n return trajectory;\n}\n\n} // namespace circles\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "acrobatic_trajectory_helper/src/heading_trajectory_helper.cpp", "content": "#include \"acrobatic_trajectory_helper/heading_trajectory_helper.h\"\n\n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace heading {\n\nvoid addConstantHeading(const double heading,\n quadrotor_common::Trajectory* trajectory) {\n auto iterator(trajectory->points.begin());\n auto iterator_prev(trajectory->points.begin());\n iterator_prev = std::prev(iterator_prev);\n auto iterator_next(trajectory->points.begin());\n iterator_next = std::next(iterator_next);\n auto last_element = trajectory->points.end();\n last_element = std::prev(last_element);\n double time_step;\n\n for (int i = 0; i < trajectory->points.size(); i++) {\n // do orientation first, since bodyrate conversion will depend on it\n Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);\n Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(\n I_eZ_I, iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81));\n\n // set full orientation and heading to zero\n Eigen::Quaternion q_heading =\n Eigen::Quaternion(Eigen::AngleAxis(\n heading, Eigen::Matrix::UnitZ()));\n Eigen::Quaternion q_orientation = quatDes * q_heading;\n iterator->orientation = q_orientation;\n iterator->heading = 0.0; // heading is now absorbed in orientation\n iterator->heading_rate = 0.0;\n iterator->heading_acceleration = 0.0;\n\n Eigen::Vector3d thrust_1;\n Eigen::Vector3d thrust_2;\n // catch case of first and last element\n if (i == 0) {\n thrust_1 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);\n time_step =\n (iterator_next->time_from_start - iterator->time_from_start).toSec();\n thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);\n } else if (i < trajectory->points.size() - 1) {\n thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);\n time_step =\n (iterator_next->time_from_start - iterator_prev->time_from_start)\n .toSec();\n thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);\n } else {\n // at the last point, we extrapolate the acceleration\n thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);\n thrust_2 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81) +\n time_step / 2.0 * iterator->jerk;\n }\n\n thrust_1.normalize();\n thrust_2.normalize();\n\n Eigen::Vector3d crossProd =\n thrust_1.cross(thrust_2); // direction of omega, in inertial axes\n Eigen::Vector3d angular_rates_wf = Eigen::Vector3d(0, 0, 0);\n if (crossProd.norm() > 0.0) {\n angular_rates_wf = std::acos(thrust_1.dot(thrust_2)) / time_step *\n crossProd / crossProd.norm();\n }\n // rotate bodyrates to bodyframe\n iterator->bodyrates = q_orientation.inverse() * angular_rates_wf;\n\n iterator_prev++;\n iterator++;\n iterator_next++;\n }\n}\n\n\nvoid addConstantHeadingRate(const double initial_heading,\n const double final_heading,\n quadrotor_common::Trajectory* trajectory) {\n if (trajectory->points.size() < 2) {\n return;\n }\n const double delta_angle =\n quadrotor_common::wrapAngleDifference(initial_heading, final_heading);\n const double trajectory_duration =\n (trajectory->points.back().time_from_start -\n trajectory->points.front().time_from_start)\n .toSec();\n\n const double heading_rate = delta_angle / trajectory_duration;\n\n std::list::iterator it;\n for (it = trajectory->points.begin(); it != trajectory->points.end(); it++) {\n const double duration_ratio =\n (it->time_from_start - trajectory->points.front().time_from_start)\n .toSec() /\n trajectory_duration;\n it->heading = initial_heading + duration_ratio * delta_angle;\n it->heading_rate = heading_rate;\n it->heading_acceleration = 0.0;\n }\n}\n\n\n\n} // namespace heading\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "acrobatic_trajectory_helper/src/polynomial_trajectory_helper.cpp", "content": "#include \"acrobatic_trajectory_helper/polynomial_trajectory_helper.h\"\n\n#include \n#include \n#include \n\nnamespace acrobatic_trajectory_helper {\n\nnamespace polynomials {\n\n// Constrained Polynomials\nquadrotor_common::Trajectory computeTimeOptimalTrajectory(\n const quadrotor_common::TrajectoryPoint& s0,\n const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::constrained_polynomial_trajectories::\n computeTimeOptimalTrajectory(s0, s1, order_of_continuity,\n max_velocity, max_normalized_thrust,\n max_roll_pitch_rate);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory computeFixedTimeTrajectory(\n const quadrotor_common::TrajectoryPoint& s0,\n const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,\n const double execution_time, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::constrained_polynomial_trajectories::\n computeFixedTimeTrajectory(s0, s1, order_of_continuity,\n execution_time);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\n// Minimum Snap Style Polynomials\nquadrotor_common::Trajectory generateMinimumSnapTrajectory(\n const Eigen::VectorXd& segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapTrajectory(segment_times, start_state, end_state,\n trajectory_settings);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory generateMinimumSnapTrajectory(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapTrajectory(initial_segment_times, start_state,\n end_state, trajectory_settings,\n max_velocity, max_normalized_thrust,\n max_roll_pitch_rate);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapTrajectoryWithSegmentRefinement(\n initial_segment_times, start_state, end_state,\n trajectory_settings);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const quadrotor_common::TrajectoryPoint& start_state,\n const quadrotor_common::TrajectoryPoint& end_state,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapTrajectoryWithSegmentRefinement(\n initial_segment_times, start_state, end_state,\n trajectory_settings, max_velocity, max_normalized_thrust,\n max_roll_pitch_rate);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory generateMinimumSnapRingTrajectory(\n const Eigen::VectorXd& segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapRingTrajectory(segment_times, trajectory_settings);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory generateMinimumSnapRingTrajectory(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapRingTrajectory(\n initial_segment_times, trajectory_settings, max_velocity,\n max_normalized_thrust, max_roll_pitch_rate);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory\ngenerateMinimumSnapRingTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapRingTrajectoryWithSegmentRefinement(\n initial_segment_times, trajectory_settings);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\nquadrotor_common::Trajectory\ngenerateMinimumSnapRingTrajectoryWithSegmentRefinement(\n const Eigen::VectorXd& initial_segment_times,\n const polynomial_trajectories::PolynomialTrajectorySettings&\n trajectory_settings,\n const double max_velocity, const double max_normalized_thrust,\n const double max_roll_pitch_rate, const double sampling_frequency) {\n polynomial_trajectories::PolynomialTrajectory polynomial =\n polynomial_trajectories::minimum_snap_trajectories::\n generateMinimumSnapRingTrajectoryWithSegmentRefinement(\n initial_segment_times, trajectory_settings, max_velocity,\n max_normalized_thrust, max_roll_pitch_rate);\n\n return samplePolynomial(polynomial, sampling_frequency);\n}\n\n// Sampling function\nquadrotor_common::Trajectory samplePolynomial(\n const polynomial_trajectories::PolynomialTrajectory& polynomial,\n const double sampling_frequency) {\n if (polynomial.trajectory_type ==\n polynomial_trajectories::TrajectoryType::UNDEFINED) {\n return quadrotor_common::Trajectory();\n }\n\n quadrotor_common::Trajectory trajectory;\n\n trajectory.points.push_back(polynomial.start_state);\n\n const ros::Duration dt(1.0 / sampling_frequency);\n ros::Duration time_from_start = polynomial.start_state.time_from_start + dt;\n\n while (time_from_start < polynomial.T) {\n trajectory.points.push_back(polynomial_trajectories::getPointFromTrajectory(\n polynomial, time_from_start));\n time_from_start += dt;\n }\n\n trajectory.points.push_back(polynomial.end_state);\n\n trajectory.trajectory_type =\n quadrotor_common::Trajectory::TrajectoryType::GENERAL;\n\n return trajectory;\n}\n\n} // namespace polynomials\n\n} // namespace acrobatic_trajectory_helper\n" }, { "path": "conda_requirements.txt", "content": "defusedxml==0.6.0\ndocutils==0.15.2\ngoogle-auth-oauthlib==0.4.1\ngoogle-pasta==0.1.7\nKeras-Applications==1.0.8\nKeras-Preprocessing==1.1.0\noauthlib==3.1.0\npyasn1==0.4.8\npydot==1.4.1\nrequests-oauthlib==1.3.0\nrsa==4.7\nscipy==1.4.1\ntensorflow-estimator==2.1.0\ntensorflow-gpu==2.4.1\nwrapt==1.11.2\n" }, { "path": "controller_learning/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(controller_learning)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-std=c++11)\nadd_compile_options(-O3)\n\n#find_package(catkin_simple REQUIRED)\n#catkin_simple(ALL_DEPS_REQUIRED)\n\n#cs_add_library(acrobatic_sequence src/acrobatic_sequence.cpp)\n\n#cs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp)\n#target_link_libraries(fpv_aggressive_trajectories acrobatic_sequence)\n#cs_add_executable(odometry_republisher src/odometry_republisher.cpp)\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rospy\n std_msgs\n message_generation\n catkin_simple REQUIRED\n)\n\ncatkin_python_setup()\ncatkin_simple()\n\n\ncs_install()\ncs_export()\n" }, { "path": "controller_learning/README.md", "content": "\n" }, { "path": "controller_learning/common.py", "content": "import os\nimport time\n\ndef update_mpc_params():\n params = {}\n params['Q_pos_xy'] = 200.0\n params['Q_pos_z'] = 500.0\n params['Q_attitude'] = 50.0\n params['Q_velocity'] = 10.0\n params['R_thrust'] = 0.1\n params['R_pitchroll'] = 0.1\n params['R_yaw'] = 0.1\n params['state_cost_exp'] = 0.0\n params['input_cost_exp'] = 0.0\n params['max_bodyrate_xy'] = 20.0\n params['max_bodyrate_z'] = 5.0\n params['min_thrust'] = 1.0\n params['max_thrust'] = 40.0\n\n print(\"Setting control parameters of MPC to:\")\n print(params)\n\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/Q_pos_xy \" + str(params['Q_pos_xy']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/Q_pos_z \" + str(params['Q_pos_z']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/Q_attitude \" + str(params['Q_attitude']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/Q_velocity \" + str(params['Q_velocity']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/R_thrust \" + str(params['R_thrust']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/R_pitchroll \" + str(params['R_pitchroll']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/R_yaw \" + str(params['R_yaw']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/state_cost_exponential \" + str(params['state_cost_exp']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/input_cost_exponential \" + str(params['input_cost_exp']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_xy \" + str(params['max_bodyrate_xy']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_z \" + str(params['max_bodyrate_z']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/min_thrust \" + str(params['min_thrust']))\n os.system(\"timeout 1s rosparam set /hummingbird/autopilot/max_thrust \" + str(params['max_thrust']))\n\n os.system(\"timeout 1s rostopic pub /hummingbird/autopilot/reload_parameters std_msgs/Empty '{}'\")\n\n\ndef setup_sim():\n print(\"==========================\")\n print(\" RESET SIMULATION \")\n print(\"==========================\")\n\n # set odometry converter back to ground truth state estimate\n # make sure fpv_aggressive_trajectories is not publishing anything!\n # turn off network\n os.system(\"timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: false'\")\n os.system(\"timeout 1s rostopic pub /hummingbird/switch_to_network std_msgs/Bool 'data: false'\")\n # after this message, autopilot will automatically go to 'BREAKING' and 'HOVER' state since\n # no control_command_inputs are published any more\n os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'\")\n os.system(\"rosservice call /gazebo/pause_physics\")\n print(\"Unpausing Physics...\")\n os.system(\"rosservice call /gazebo/unpause_physics\")\n print(\"Placing quadrotor...\")\n os.system(\"timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty\")\n os.system(\"rosservice call /gazebo/set_model_state \"\n \"'{model_state: { model_name: hummingbird, pose: { position: { x: 0.0, y: 0.0 ,z: 0.2 }, \"\n \"orientation: {x: 0, y: 0, z: 0.0, w: 1.0 }}, \"\n \"twist:{ linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 }}, \"\n \"reference_frame: world } }'\")\n time.sleep(2)\n # start quadrotor\n # os.system(\"timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'\")\n # os.system(\"timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty\")\n\n\ndef random_replace():\n reset_success_str = 'rostopic pub /success_reset std_msgs/Empty \"{}\" -1'\n os.system(reset_success_str)\n\n\ndef initialize_vio():\n # Make sure to use GT odometry in this step\n os.system(\"timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty\")\n os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'\")\n\n # reset quad to initial position\n os.system(\"timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'\")\n print(\"Start quadrotor\")\n os.system(\"timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty\")\n time.sleep(10)\n # Restart VIO\n os.system(\"timeout 1s rostopic pub /feature_tracker/restart std_msgs/Bool 'data: true' \")\n os.system(\n \"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} } }'\")\n time.sleep(9)\n # TODO: maybe remove this\n # os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Bool 'data: true'\")\n return\n" }, { "path": "controller_learning/config/dagger_settings.yaml", "content": "log_dir: \"results/loop\"\nquad_name: \"hummingbird\"\nverbose: False\nseq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency.\ncheckpoint:\n # Put to true and add a path for loading a ckpt\n resume_training: False\n resume_file: \"\"\ndata_generation:\n max_rollouts: 150\n train_every_n_rollouts: 30\n double_th_every_n_rollouts: 30\ntrain:\n max_training_epochs: 20\n max_allowed_error: 0.6 # Collision rollouts are eliminated from the training\n min_number_fts: 40 # Number of feature tracks per image\n batch_size: 32\n summary_freq: 400\n train_dir: \"data/loop/train\" # Where data will be generated and where the network will train\n val_dir: \"data/loop/test\" # Validation Data, change to a new data folder!\n save_every_n_epochs: 5\n use_imu: True\n use_fts_tracks: True\ntest_time:\n execute_nw_predictions: True\n # Dagger constants and random controller constants for exploration\n fallback_threshold_rates: 1.0\n rand_thrust_mag: 6\n rand_rate_mag: 3.5\n rand_controller_prob: 0.05\n\n" }, { "path": "controller_learning/config/settings.py", "content": "import math\nimport os\nimport shutil\nimport sys\nimport time\nimport datetime\n\nimport yaml\n\n\ndef create_settings(settings_yaml, mode='test'):\n setting_dict = {'train': TrainSetting,\n 'test': TestSetting,\n 'dagger': DaggerSetting}\n settings = setting_dict.get(mode, None)\n if settings is None:\n raise IOError(\"Unidentified Settings\")\n settings = settings(settings_yaml)\n return settings\n\n\nclass Settings:\n def __init__(self, settings_yaml, generate_log=True):\n assert os.path.isfile(settings_yaml), settings_yaml\n\n with open(settings_yaml, 'r') as stream:\n settings = yaml.safe_load(stream)\n\n self.quad_name = settings['quad_name']\n\n self.seq_len = settings['seq_len']\n\n # --- checkpoint ---\n checkpoint = settings['checkpoint']\n self.resume_training = checkpoint['resume_training']\n assert isinstance(self.resume_training, bool)\n self.resume_ckpt_file = checkpoint['resume_file']\n\n # Save a copy of the parameters for reproducibility\n log_root = settings['log_dir']\n if not log_root == '' and generate_log:\n current_time = datetime.datetime.now().strftime(\"%Y%m%d-%H%M%S\")\n self.log_dir = os.path.join(log_root, current_time)\n os.makedirs(self.log_dir)\n net_file = \"./src/ControllerLearning/models/nets.py\"\n assert os.path.isfile(net_file)\n shutil.copy(net_file, self.log_dir)\n shutil.copy(settings_yaml, self.log_dir)\n\n def add_flags(self):\n self._add_flags()\n\n def _add_flags(self):\n raise NotImplementedError\n\n\nclass TrainSetting(Settings):\n def __init__(self, settings_yaml):\n super(TrainSetting, self).__init__(settings_yaml, generate_log=True)\n self.settings_yaml = settings_yaml\n self.add_flags()\n\n def _add_flags(self):\n with open(self.settings_yaml, 'r') as stream:\n settings = yaml.safe_load(stream)\n # --- Train Time --- #\n train_conf = settings['train']\n self.max_training_epochs = train_conf['max_training_epochs']\n self.max_allowed_error = train_conf['max_allowed_error']\n self.batch_size = train_conf['batch_size']\n self.summary_freq = train_conf['summary_freq']\n self.train_dir = train_conf['train_dir']\n self.use_fts_tracks = train_conf['use_fts_tracks']\n self.use_imu = train_conf['use_imu']\n self.val_dir = train_conf['val_dir']\n self.min_number_fts = train_conf['min_number_fts']\n self.save_every_n_epochs = train_conf['save_every_n_epochs']\n\n\nclass TestSetting(Settings):\n def __init__(self, settings_yaml):\n super(TestSetting, self).__init__(settings_yaml, generate_log=True)\n self.settings_yaml = settings_yaml\n self.add_flags()\n\n def _add_flags(self):\n with open(self.settings_yaml, 'r') as stream:\n settings = yaml.safe_load(stream)\n test_time = settings['test_time']\n self.execute_nw_predictions = test_time['execute_nw_predictions']\n assert isinstance(self.execute_nw_predictions, bool)\n self.max_rollouts = test_time['max_rollouts']\n self.fallback_threshold_rates = test_time['fallback_threshold_rates']\n self.fallback_threshold_thrust = test_time['fallback_threshold_thrust']\n self.min_number_fts = test_time['min_number_fts']\n self.use_imu = test_time['use_imu']\n self.use_fts_tracks = test_time['use_fts_tracks']\n self.verbose = settings['verbose']\n\n\nclass DaggerSetting(Settings):\n def __init__(self, settings_yaml):\n super(DaggerSetting, self).__init__(settings_yaml, generate_log=True)\n self.settings_yaml = settings_yaml\n self.add_flags()\n\n def _add_flags(self):\n with open(self.settings_yaml, 'r') as stream:\n settings = yaml.safe_load(stream)\n # --- Data Generation --- #\n data_gen = settings['data_generation']\n self.max_rollouts = data_gen['max_rollouts']\n self.double_th_every_n_rollouts = data_gen['double_th_every_n_rollouts']\n self.train_every_n_rollouts = data_gen['train_every_n_rollouts']\n # --- Test Time --- #\n test_time = settings['test_time']\n self.execute_nw_predictions = test_time['execute_nw_predictions']\n assert isinstance(self.execute_nw_predictions, bool)\n self.fallback_threshold_rates = test_time['fallback_threshold_rates']\n self.rand_thrust_mag = test_time['rand_thrust_mag']\n self.rand_rate_mag = test_time['rand_rate_mag']\n self.rand_controller_prob = test_time['rand_controller_prob']\n # --- Train Time --- #\n train_conf = settings['train']\n self.max_training_epochs = train_conf['max_training_epochs']\n self.max_allowed_error = train_conf['max_allowed_error']\n self.batch_size = train_conf['batch_size']\n self.min_number_fts = train_conf['min_number_fts']\n self.summary_freq = train_conf['summary_freq']\n self.train_dir = train_conf['train_dir']\n self.val_dir = train_conf['val_dir']\n self.use_imu = train_conf['use_imu']\n self.use_fts_tracks = train_conf['use_fts_tracks']\n self.val_dir = train_conf['val_dir']\n self.save_every_n_epochs = train_conf['save_every_n_epochs']\n self.verbose = settings['verbose']\n assert isinstance(self.verbose, bool)\n" }, { "path": "controller_learning/config/test_settings.yaml", "content": "quad_name: 'hummingbird'\nlog_dir: '/tmp/test'\nideal_inputs: False\nseq_len: 1\nverbose: False\nthrow_params:\n vlin_range: 5\n vang_range: 5\n pos_range: 5\ncheckpoint:\n resume_training: True\n resume_file: \"ckpt/loop/fts_and_imu/ckpt-7\"\ntest_time:\n max_rollouts: 5\n min_number_fts: 40\n execute_nw_predictions: True\n fallback_threshold_thrust: 100\n fallback_threshold_rates: 100\n use_imu: True\n use_fts_tracks: True\n" }, { "path": "controller_learning/config/train_settings.yaml", "content": "log_dir: 'results/train_fma_cork_seq_1_fts_tracks_newcam_new_end'\nquad_name: 'none'\nseq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency.\ncheckpoint:\n # Put to true and add a path for loading a ckpt\n resume_training: False\n resume_file: \"\"\ntrain:\n max_training_epochs: 150\n max_allowed_error: 0.7\n batch_size: 32\n summary_freq: 400\n min_number_fts: 40\n train_dir: \"data/loop/train\"\n val_dir: \"data/loop/val\"\n save_every_n_epochs: 5\n use_imu: True\n use_fts_tracks: True\n" }, { "path": "controller_learning/data/.gitignore", "content": "*\n*/\n!.gitignore" }, { "path": "controller_learning/iterative_learning_trajectories.py", "content": "#!/usr/bin/env python3\n\nimport argparse\nimport os\nimport sys\nimport time\nimport numpy as np\nimport rospy\nfrom ControllerLearning import TrajectoryLearning\nfrom std_msgs.msg import Bool\nfrom common import update_mpc_params, setup_sim, random_replace, initialize_vio\n\nfrom config.settings import create_settings\n\n\nclass Trainer():\n def __init__(self, settings):\n rospy.init_node('iterative_learning_node', anonymous=False)\n self.settings = settings\n self.trajectory_done = False\n self.traj_done_sub = rospy.Subscriber(\"/hummingbird/switch_to_network\", Bool,\n self.callback_traj_done, queue_size=1)\n\n def callback_traj_done(self, data):\n self.trajectory_done = data.data\n\n def start_experiment(self, learner):\n reset_success_str = 'rostopic pub /success_reset std_msgs/Empty \"{}\" -1'\n os.system(reset_success_str)\n initialize_vio()\n learner.latest_thrust_factor = 1.0 # Could be changed to adapt for different quadrotors.\n print(\"Doing experiment {}, with such factor {}\".format(learner.rollout_idx, learner.latest_thrust_factor))\n # if True, we will still use the VIO-orientation, even when initialization is poor.\n # If set to False, we will fall back to GT.\n # Set to True only if you are sure your VIO is well calibrated.\n use_chimaera = False\n # check if initialization was good. If not, we will perform rollout with ground truth to not waste time!\n vio_init_good = learner.vio_init_good\n if vio_init_good:\n rospy.loginfo(\"VINS-Mono initialization is good, switching to vision-based state estimate!\")\n os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 1'\")\n else:\n if use_chimaera:\n rospy.logerr(\"VINS-Mono initialization is poor, use orientation, bodyrates from VIO and linear velocity estimate from GT!\")\n os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 2'\")\n else:\n rospy.logerr(\"VINS-Mono initialization is poor, keeping ground truth estimate!\")\n os.system(\"timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'\")\n # Start Flying!\n os.system(\"timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: true'\")\n return vio_init_good\n\n def perform_training(self):\n learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode=\"iterative\")\n shutdown_requested = False\n train_every_n_rollouts = self.settings.train_every_n_rollouts\n if self.settings.execute_nw_predictions:\n print(\"-------------------------------------------\")\n print(\"Running Dagger with the following params\")\n print(\"Rates threshold: {}; Rand Controller Th {}\".format(\n self.settings.fallback_threshold_rates,\n self.settings.rand_controller_prob))\n print(\"-------------------------------------------\")\n else:\n print(\"---------------------------\")\n print(\"Collecting Data with Expert\")\n print(\"---------------------------\")\n while (not shutdown_requested) and (learner.rollout_idx < self.settings.max_rollouts):\n self.trajectory_done = False\n setup_sim()\n learner.start_data_recording()\n self.start_experiment(learner)\n print(\"Starting Experiment {}\".format(learner.rollout_idx))\n start_time = time.time()\n time_run = 0\n ref_log = []\n gt_pos_log = []\n error_log = []\n while (not self.trajectory_done) and (time_run < 100):\n time.sleep(0.1)\n time_run = time.time() - start_time\n if learner.use_network and learner.reference_updated:\n pos_ref_dict = learner.compute_trajectory_error()\n gt_pos_log.append(pos_ref_dict[\"gt_pos\"])\n ref_log.append(pos_ref_dict[\"gt_ref\"])\n error_log.append(np.linalg.norm(pos_ref_dict[\"gt_pos\"] - pos_ref_dict[\"gt_ref\"]))\n # final logging\n tracking_error = np.mean(error_log)\n median_traj_error = np.median(error_log)\n t_log = np.stack((ref_log, gt_pos_log), axis=0)\n expert_usage = learner.stop_data_recording()\n shutdown_requested = learner.shutdown_requested()\n print(\"Expert used {:.03f}% of the times\".format(100.0 * expert_usage))\n print(\"Mean Tracking Error is {:.03f}\".format(tracking_error))\n print(\"Median Tracking Error is {:.03f}\".format(median_traj_error))\n if learner.rollout_idx % train_every_n_rollouts == 0:\n os.system(\"rosservice call /gazebo/pause_physics\")\n learner.train()\n os.system(\"rosservice call /gazebo/unpause_physics\")\n if (learner.rollout_idx % self.settings.double_th_every_n_rollouts) == 0:\n self.settings.fallback_threshold_rates += 0.5\n print(\"Setting Rate Threshold to {}\".format(self.settings.fallback_threshold_rates))\n self.settings.rand_controller_prob = np.minimum(0.3, self.settings.rand_controller_prob * 2)\n print(\"Setting Rand Controller Prob to {}\".format(self.settings.rand_controller_prob))\n if self.settings.verbose:\n t_log_fname = os.path.join(self.settings.log_dir, \"traj_log_{:5d}.npy\".format(learner.rollout_idx))\n np.save(t_log_fname, t_log)\n\n def perform_testing(self):\n learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode=\"testing\")\n shutdown_requested = False\n rollout_idx = 0\n while (not shutdown_requested) and (rollout_idx < self.settings.max_rollouts):\n self.trajectory_done = False\n setup_sim()\n if self.settings.verbose:\n # Will save data for debugging reasons\n learner.start_data_recording()\n self.start_experiment(learner)\n start_time = time.time()\n time_run = 0\n ref_log = []\n gt_pos_log = []\n error_log = []\n while (not self.trajectory_done) and (time_run < 100):\n time.sleep(0.1)\n time_run = time.time() - start_time\n if learner.use_network and learner.reference_updated:\n pos_ref_dict = learner.compute_trajectory_error()\n gt_pos_log.append(pos_ref_dict[\"gt_pos\"])\n ref_log.append(pos_ref_dict[\"gt_ref\"])\n error_log.append(np.linalg.norm(pos_ref_dict[\"gt_pos\"] - pos_ref_dict[\"gt_ref\"]))\n # final logging\n tracking_error = np.mean(error_log)\n median_traj_error = np.median(error_log)\n t_log = np.stack((ref_log, gt_pos_log), axis=0)\n expert_usage = learner.stop_data_recording()\n shutdown_requested = learner.shutdown_requested()\n print(\"{} Rollout: Expert used {:.03f}% of the times\".format(rollout_idx+1, 100.0 * expert_usage))\n print(\"Mean Tracking Error is {:.03f}\".format(tracking_error))\n print(\"Median Tracking Error is {:.03f}\".format(median_traj_error))\n rollout_idx +=1\n if self.settings.verbose:\n t_log_fname = os.path.join(self.settings.log_dir, \"traj_log_{:05d}.npy\".format(rollout_idx))\n np.save(t_log_fname, t_log)\n\n\ndef main():\n parser = argparse.ArgumentParser(description='Train RAF network.')\n parser.add_argument('--settings_file', help='Path to settings yaml', required=True)\n\n args = parser.parse_args()\n settings_filepath = args.settings_file\n settings = create_settings(settings_filepath, mode='dagger')\n update_mpc_params()\n setup_sim()\n trainer = Trainer(settings)\n trainer.perform_training()\n\n\nif __name__ == \"__main__\":\n main()\n" }, { "path": "controller_learning/launch/simulation/base_quad_simulation_no_controller.launch", "content": "\n\n\n \n \n \n \n \n \n\n \n \n \n \n \n\n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n\n \n \n\n \n \n \n\n \n \n \n\n \n\n \n\n\n" }, { "path": "controller_learning/launch/simulation/controller.launch", "content": "\n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/custom_rotors_interface.launch", "content": "\n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/minisim.launch", "content": "\n\n\n \n \n \n\n \n \n \n \n\n \n \n\n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n \n\n \n\n \n\n\n\n" }, { "path": "controller_learning/launch/simulation/quadrotor_empty_world_no_controller.launch", "content": "\n\n \n\n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/quadrotor_rgb_world_no_controller.launch", "content": "\n\n \n\n \n \n \n\n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/rpg_mpc.launch", "content": "\n\n\n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/rpg_pid.launch", "content": "\n\n\n \n \n \n \n\n \n \n \n \n \n \n\n \n\n \n \n \n \n \n \n \n \n\n \n\n \n \n \n \n \n\n \n\n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/rpg_rotors_interface.launch", "content": "\n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "controller_learning/launch/simulation/rviz_only.launch", "content": "\n\n\n \n \n \n\n\n\n \n\n\n \n\n\n \n\n\n\n" }, { "path": "controller_learning/launch/simulation/simulation.launch", "content": "\n\n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n\n \n\n \n\n \n\n\n\n" }, { "path": "controller_learning/package.xml", "content": "\n\n controller_learning\n 0.0.0\n The controller_learning package\n\n Elia Kaufmann\n TODO\n\n Elia Kaufmann\n\n catkin\n catkin_simple\n\n autopilot\n eigen_catkin\n minimum_jerk_trajectories\n quadrotor_common\n quadrotor_msgs\n roscpp\n std_msgs\n nav_msgs\n\n \n \n \n\n" }, { "path": "controller_learning/results/.gitignore", "content": "*\n*/\n!.gitignore\n" }, { "path": "controller_learning/setup.py", "content": "#!/usr/bin/env python\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\n\nd = generate_distutils_setup(\n packages=['ControllerLearning'],\n package_dir={'': 'src'}\n)\n\nsetup(**d)\n" }, { "path": "controller_learning/src/ControllerLearning/TrajectoryBase.py", "content": "#!/usr/bin/env python3\nimport collections\nimport copy\nimport csv\nimport datetime\nimport os\nimport random\n\nimport numpy as np\nimport rospy\nfrom nav_msgs.msg import Odometry\nfrom quadrotor_msgs.msg import ControlCommand\nfrom quadrotor_msgs.msg import TrajectoryPoint\nfrom sensor_msgs.msg import PointCloud\nfrom std_msgs.msg import Bool\nfrom std_msgs.msg import Empty\nfrom scipy.spatial.transform import Rotation as R\n\nfrom .models.bodyrate_learner import BodyrateLearner\n\nTRACK_NUM_NORMALIZE = 10 # Normalization factor for feature length\n\nclass TrajectoryBase(object):\n def __init__(self, config, mode):\n self.config = config\n self.odometry = Odometry()\n self.gt_odometry = Odometry()\n self.rows_buffer = []\n self.ref_state = TrajectoryPoint()\n self.vins_odometry = None\n self.gt_control_command = ControlCommand()\n self.counter = 0\n self.features = None\n self.image = None\n self.images_input = None\n self.maneuver_complete = False\n self.shutdown_node = False\n self.record_data = False\n self.is_training = False\n self.use_network = False\n self.net_initialized = False\n self.reference_updated = False\n self.rollout_idx = 0\n self.n_times_net = 0.001 # Due to cope against weird gazebo behaviour\n self.n_times_expert = 0\n self.mode = mode\n self.fts_queue = collections.deque([], maxlen=self.config.seq_len)\n self.state_queue = collections.deque([], maxlen=self.config.seq_len)\n self.reset_queue()\n self.learner = BodyrateLearner(settings=config)\n if self.mode == 'training':\n return # Nothing to initialize\n self.pub_actions = rospy.Publisher(\"/\" + self.config.quad_name + \"/control_command\",\n ControlCommand, queue_size=1)\n self.odometry_sub = rospy.Subscriber(\"/\" + self.config.quad_name + \"/state_estimate\",\n Odometry,\n self.callback_odometry,\n queue_size=1,\n tcp_nodelay=True)\n self.shutdown_sub = rospy.Subscriber(\"shutdown_learner\", Empty,\n self.callback_shutdown,\n queue_size=1)\n self.ref_sub = rospy.Subscriber(\"/\" + self.config.quad_name + \"/vio_reference\",\n TrajectoryPoint,\n self.callback_ref,\n queue_size=1,\n tcp_nodelay=True)\n self.control_command_sub = rospy.Subscriber(\"/\" + self.config.quad_name + \"/control_command_label\",\n ControlCommand,\n self.callback_control_command, queue_size=1,\n tcp_nodelay=True)\n if self.config.use_fts_tracks or self.mode == 'iterative':\n self.fts_sub = rospy.Subscriber(\"/feature_tracker/feature\", PointCloud,\n self.callback_fts, queue_size=1)\n self.traj_done_sub = rospy.Subscriber(\"/\" + self.config.quad_name + \"/switch_to_network\", Bool,\n self.callback_nw_switch, queue_size=1)\n self.trajectory_start = rospy.Subscriber(\"/\" + self.config.quad_name + \"/trajectory_computation_finish\", Bool,\n self.callback_start_trajectory, queue_size=10)\n if self.mode == \"testing\":\n self.success = 1\n\n def start_data_recording(self):\n print(\"Collecting data\")\n self.record_data = True\n\n def stop_data_recording(self):\n print(\"Stop data collection\")\n self.record_data = False\n expert_usage = self.n_times_expert / (self.n_times_net + self.n_times_expert)\n return expert_usage\n\n def reset_queue(self):\n self.fts_queue.clear()\n self.state_queue.clear()\n self.ref_rot = [0 for _ in range(9)]\n self.odom_rot = [0 for _ in range(9)]\n init_dict = {}\n if self.config.use_imu:\n n_init_states = 30\n else:\n n_init_states = 15\n for i in range(self.config.min_number_fts):\n init_dict[i] = np.zeros((5,), dtype=np.float32)\n for _ in range(self.config.seq_len):\n self.fts_queue.append(init_dict)\n self.state_queue.append(np.zeros((n_init_states,)))\n self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \\\n for j in range(self.config.seq_len)])\n\n def publish_control_command(self, control_command):\n self.pub_actions.publish(control_command)\n\n def preprocess_fts(self, data):\n features_dict = {}\n for i in range(len(data.points)):\n ft_id = data.channels[0].values[i]\n x = data.points[i].x\n y = data.points[i].y\n z = data.points[i].z\n velocity_x = data.channels[3].values[i]\n velocity_y = data.channels[4].values[i]\n track_count = 2 * (data.channels[5].values[i] / TRACK_NUM_NORMALIZE) - 1\n assert z == 1\n feat = np.array([x, y, velocity_x, velocity_y, track_count])\n features_dict[ft_id] = feat\n return features_dict\n\n def add_missing_fts(self, features_dict):\n processed_dict = copy.copy(features_dict)\n # Could be both positive or negative\n missing_fts = self.config.min_number_fts - len(features_dict.keys())\n if missing_fts > 0:\n # Features are missing\n if missing_fts != self.config.min_number_fts:\n # There is something, we can sample\n new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts))\n for j in range(missing_fts):\n processed_dict[-j - 1] = features_dict[new_features_keys[j]]\n else:\n raise IOError(\"There should not be zero features!\")\n elif missing_fts < 0:\n # There are more features than we need, so sample\n del_features_keys = random.sample(features_dict.keys(), int(-missing_fts))\n for k in del_features_keys:\n del processed_dict[k]\n return processed_dict\n\n def callback_fts(self, data):\n if (not self.config.use_fts_tracks) and (self.mode == 'testing'):\n return\n features = self.preprocess_fts(data)\n if len(features.keys()) != 0:\n # Update features only if something is available\n self.features = features\n preprocessed_fts = self.add_missing_fts(self.features)\n self.fts_queue.append(preprocessed_fts)\n self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \\\n for j in range(self.config.seq_len)])\n\n def callback_shutdown(self, data):\n self.shutdown_node = True\n\n def callback_nw_switch(self, msg):\n self.use_network = False\n if msg.data:\n # Trajectory is done, stop everything\n self.maneuver_complete = True\n print(\"Maneuver is finished\")\n\n def callback_start_trajectory(self, data):\n # VIO is ready, can fly new trajectory\n if data.data:\n print(\"Ready to start trajectory, network on\")\n self.use_network = True\n self.counter = 0\n\n def callback_start(self, data):\n print(\"Callback START\")\n self.pipeline_off = False\n\n def callback_off(self, data):\n print(\"Callback OFF\")\n self.pipeline_off = True\n\n def maneuver_finished(self):\n return self.maneuver_complete\n\n def callback_odometry(self, data):\n self.odometry = data\n self.odom_rot = R.from_quat([self.odometry.pose.pose.orientation.x,\n self.odometry.pose.pose.orientation.y,\n self.odometry.pose.pose.orientation.z,\n self.odometry.pose.pose.orientation.w]).as_matrix().reshape((9,)).tolist()\n\n def callback_gt_odometry(self, data):\n self.gt_odometry = data\n\n def callback_ref(self, data):\n self.ref_state = data\n self.ref_rot = R.from_quat([self.ref_state.pose.orientation.x,\n self.ref_state.pose.orientation.y,\n self.ref_state.pose.orientation.z,\n self.ref_state.pose.orientation.w]).as_matrix().reshape((9,)).tolist()\n if not self.reference_updated:\n self.reference_updated = True\n\n def callback_control_command(self, data):\n self.control_command = data\n self._generate_control_command()\n\n def shutdown_requested(self):\n return self.shutdown_node\n\n def _prepare_net_inputs(self):\n if not self.net_initialized:\n # return fake input for init\n # TODO: change to features\n if self.config.use_imu:\n n_init_states = 30\n else:\n n_init_states = 15\n inputs = {'fts': np.zeros((1, self.config.seq_len, 40, 5), dtype=np.float64),\n 'state': np.zeros((1, self.config.seq_len, n_init_states),\n dtype=np.float64)}\n return inputs\n\n # Reference\n state_inputs = self.ref_rot + [ self.ref_state.velocity.linear.x,\n self.ref_state.velocity.linear.y,\n self.ref_state.velocity.linear.z,\n self.ref_state.velocity.angular.x,\n self.ref_state.velocity.angular.y,\n self.ref_state.velocity.angular.z]\n\n if self.config.use_imu:\n\n imu_states = self.odom_rot + [\n self.odometry.twist.twist.linear.x,\n self.odometry.twist.twist.linear.y,\n self.odometry.twist.twist.linear.z,\n self.odometry.twist.twist.angular.x,\n self.odometry.twist.twist.angular.y,\n self.odometry.twist.twist.angular.z]\n\n state_inputs = imu_states + state_inputs\n\n state_inputs = np.array(state_inputs)\n\n self.state_queue.append(state_inputs)\n state_inputs = np.stack(self.state_queue, axis=0)\n inputs = {'fts': np.expand_dims(self.features_input, axis=0),\n 'state': np.expand_dims(state_inputs, axis=0)}\n return inputs\n\n def _generate_control_command(self):\n pass # Implemented in derived class\n\n def write_csv_header(self):\n pass # Implemented in derived class\n" }, { "path": "controller_learning/src/ControllerLearning/TrajectoryLearning.py", "content": "#!/usr/bin/env python3\nimport csv\nimport datetime\nimport os\nimport random\n\nimport numpy as np\nimport rospy\nfrom nav_msgs.msg import Odometry\nfrom quadrotor_msgs.msg import ControlCommand\nfrom std_msgs.msg import Bool\nfrom std_msgs.msg import Empty\n\nfrom .TrajectoryBase import TrajectoryBase, TRACK_NUM_NORMALIZE\n\n\n\nclass TrajectoryLearning(TrajectoryBase):\n def __init__(self, config, mode):\n super(TrajectoryLearning, self).__init__(config, mode)\n self.gt_odometry = Odometry()\n self.latest_thrust_factor = 1.0 # Init, can be changed for different drones\n self.recorded_samples = 0\n if self.mode == 'training':\n return # Nothing to initialize\n self.pub_reset_vio = rospy.Publisher(\"/feature_tracker/restart\",\n Bool, queue_size=1)\n self.success_subs = rospy.Subscriber(\"success_reset\", Empty,\n self.callback_success_reset,\n queue_size=1)\n self.state_estimate_sub = rospy.Subscriber(\"/hummingbird/odometry_converted_vio\",\n Odometry,\n self.callback_odometry,\n queue_size=1,\n tcp_nodelay=True)\n self.ground_truth_odom = rospy.Subscriber(\"/hummingbird/ground_truth/odometry\",\n Odometry,\n self.callback_gt_odometry,\n queue_size=1)\n self.vins_mono_sub = rospy.Subscriber(\"/vins_estimator/imu_propagate\", Odometry,\n self.callback_vins_mono, queue_size=1)\n\n if mode == \"iterative\" or self.config.verbose:\n self.write_csv_header()\n if self.mode == \"testing\":\n self.success = 1\n\n def train(self):\n self.is_training = True\n self.learner.train()\n self.is_training = False\n self.use_network = False\n\n def callback_success_reset(self, data):\n print(\"Received call to Clear Buffer and Restart Experiment\")\n os.system(\"rosservice call /gazebo/pause_physics\")\n self.rollout_idx += 1\n self.reset_queue()\n print('Buffer Cleared')\n if self.mode == 'testing':\n self.success = 1 # We are positive, default is pass\n # Init is hacky, but gazebo is very bad!\n self.n_times_expert = 0.000\n self.n_times_net = 0.001\n self.reference_updated = False\n os.system(\"rosservice call /gazebo/unpause_physics\")\n print('Done Reset')\n\n def callback_gt_odometry(self, data):\n self.gt_odometry = data\n\n def callback_vins_mono(self, data):\n self.vins_odometry = data\n\n @property\n def vio_init_good(self):\n max_allowed_velocity = 0.1\n if abs(self.vins_odometry.twist.twist.linear.x) < max_allowed_velocity and \\\n abs(self.vins_odometry.twist.twist.linear.y) < max_allowed_velocity and \\\n abs(self.vins_odometry.twist.twist.linear.z) < max_allowed_velocity:\n return True\n else:\n return False\n\n def save_data(self):\n row = [self.rollout_idx,\n self.odometry.header.stamp.to_sec(),\n # GT Positon\n self.gt_odometry.pose.pose.position.x,\n self.gt_odometry.pose.pose.position.y,\n self.gt_odometry.pose.pose.position.z,\n self.ref_state.pose.position.z - self.gt_odometry.pose.pose.position.z,\n self.gt_odometry.pose.pose.orientation.w,\n self.gt_odometry.pose.pose.orientation.x,\n self.gt_odometry.pose.pose.orientation.y,\n self.gt_odometry.pose.pose.orientation.z,\n self.gt_odometry.twist.twist.linear.x,\n self.gt_odometry.twist.twist.linear.y,\n self.gt_odometry.twist.twist.linear.z,\n self.gt_odometry.twist.twist.angular.x,\n self.gt_odometry.twist.twist.angular.y,\n self.gt_odometry.twist.twist.angular.z,\n # VIO Estimate\n self.odometry.pose.pose.position.x,\n self.odometry.pose.pose.position.y,\n self.odometry.pose.pose.position.z,\n self.ref_state.pose.position.z - self.odometry.pose.pose.position.z,\n self.odometry.pose.pose.orientation.w,\n self.odometry.pose.pose.orientation.x,\n self.odometry.pose.pose.orientation.y,\n self.odometry.pose.pose.orientation.z,\n self.odometry.twist.twist.linear.x,\n self.odometry.twist.twist.linear.y,\n self.odometry.twist.twist.linear.z,\n self.odometry.twist.twist.angular.x,\n self.odometry.twist.twist.angular.y,\n self.odometry.twist.twist.angular.z,\n # Reference state\n self.ref_state.pose.position.x,\n self.ref_state.pose.position.y,\n self.ref_state.pose.position.z,\n self.ref_state.pose.orientation.w,\n self.ref_state.pose.orientation.x,\n self.ref_state.pose.orientation.y,\n self.ref_state.pose.orientation.z,\n self.ref_state.velocity.linear.x,\n self.ref_state.velocity.linear.y,\n self.ref_state.velocity.linear.z,\n self.ref_state.velocity.angular.x,\n self.ref_state.velocity.angular.y,\n self.ref_state.velocity.angular.z,\n # MPC output with GT Position\n self.control_command.collective_thrust,\n self.control_command.bodyrates.x,\n self.control_command.bodyrates.y,\n self.control_command.bodyrates.z,\n # NET output with GT Position\n self.net_control.collective_thrust,\n self.net_control.bodyrates.x,\n self.net_control.bodyrates.y,\n self.net_control.bodyrates.z,\n # Maneuver type\n 0]\n\n if self.record_data and self.gt_odometry.pose.pose.position.z > 0.3 and \\\n self.control_command.collective_thrust > 0.2:\n with open(self.csv_filename, 'a') as writeFile:\n writer = csv.writer(writeFile)\n writer.writerows([row])\n fts_name = '{:08d}.npy'\n fts_filename = os.path.join(self.image_save_dir,\n fts_name.format(self.recorded_samples))\n np.save(fts_filename, self.features)\n self.recorded_samples += 1\n\n def callback_control_command(self, data):\n self.control_command = data\n self._generate_control_command()\n if self.mode == 'testing' and self.gt_odometry.pose.pose.position.z < 0.3:\n self.success = 0\n\n def compute_trajectory_error(self):\n gt_ref = np.array([self.ref_state.pose.position.x,\n self.ref_state.pose.position.y,\n self.ref_state.pose.position.z])\n gt_pos = np.array([self.gt_odometry.pose.pose.position.x,\n self.gt_odometry.pose.pose.position.y,\n self.gt_odometry.pose.pose.position.z])\n results = {\"gt_ref\": gt_ref, \"gt_pos\": gt_pos}\n return results\n\n def publish_control_command(self, control_command):\n control_command.collective_thrust = self.latest_thrust_factor * control_command.collective_thrust\n self.pub_actions.publish(control_command)\n\n def _generate_control_command(self):\n inputs = self._prepare_net_inputs()\n if not self.net_initialized:\n # Apply Network to init\n results = self.learner.inference(inputs)\n print(\"Net initialized\")\n self.net_initialized = True\n self.publish_control_command(self.control_command)\n\n if not self.use_network or not self.reference_updated \\\n or (len(inputs['fts'].shape) != 4):\n # Will be in here if:\n # - starting and VIO init\n # - Image queue is not ready, can only run expert\n self.publish_control_command(self.control_command)\n if self.use_network:\n print(\"Using expert wait for ref\")\n return\n # Use always expert at the beginning (approximately 0.2s) to avoid syncronization problems\n if self.counter < 10:\n self.counter += 1\n self.publish_control_command(self.control_command)\n return\n\n # Apply Network\n results = self.learner.inference(inputs)\n control_command = ControlCommand()\n control_command.armed = True\n control_command.expected_execution_time = rospy.Time.now()\n control_command.control_mode = 2\n control_command.collective_thrust = results[0][0].numpy()\n control_command.bodyrates.x = results[0][1].numpy()\n control_command.bodyrates.y = results[0][2].numpy()\n control_command.bodyrates.z = results[0][3].numpy()\n self.net_control = control_command\n\n # Log immediately everything to avoid surprises (if required)\n if self.record_data:\n self.save_data()\n\n # Apply random controller now and then to facilitate exploration\n if (self.mode != 'testing') and random.random() < self.config.rand_controller_prob:\n self.control_command.collective_thrust += self.config.rand_thrust_mag * (random.random() - 0.5) * 2\n self.control_command.bodyrates.x += self.config.rand_rate_mag * (random.random() - 0.5) * 2\n self.control_command.bodyrates.y += self.config.rand_rate_mag * (random.random() - 0.5) * 2\n self.control_command.bodyrates.z += self.config.rand_rate_mag * (random.random() - 0.5) * 2\n self.publish_control_command(self.control_command)\n return\n\n # Dagger (on control command label).\n d_thrust = control_command.collective_thrust - self.control_command.collective_thrust\n d_br_x = control_command.bodyrates.x - self.control_command.bodyrates.x\n d_br_y = control_command.bodyrates.y - self.control_command.bodyrates.y\n d_br_z = control_command.bodyrates.z - self.control_command.bodyrates.z\n if self.config.execute_nw_predictions \\\n and abs(d_thrust) < self.config.fallback_threshold_rates \\\n and abs(d_br_x) < self.config.fallback_threshold_rates \\\n and abs(d_br_y) < self.config.fallback_threshold_rates \\\n and abs(d_br_z) < self.config.fallback_threshold_rates:\n self.n_times_net += 1\n self.publish_control_command(control_command)\n else:\n self.n_times_expert += 1\n self.publish_control_command(self.control_command)\n\n def write_csv_header(self):\n row = [\"Rollout_idx\",\n \"Odometry_stamp\",\n # GT Position\n \"gt_Position_x\",\n \"gt_Position_y\",\n \"gt_Position_z\",\n \"gt_Position_z_error\",\n \"gt_Orientation_w\",\n \"gt_Orientation_x\",\n \"gt_Orientation_y\",\n \"gt_Orientation_z\",\n \"gt_V_linear_x\",\n \"gt_V_linear_y\",\n \"gt_V_linear_z\",\n \"gt_V_angular_x\",\n \"gt_V_angular_y\",\n \"gt_V_angular_z\",\n # VIO Estimate\n \"Position_x\",\n \"Position_y\",\n \"Position_z\",\n \"Position_z_error\",\n \"Orientation_w\",\n \"Orientation_x\",\n \"Orientation_y\",\n \"Orientation_z\",\n \"V_linear_x\",\n \"V_linear_y\",\n \"V_linear_z\",\n \"V_angular_x\",\n \"V_angular_y\",\n \"V_angular_z\",\n # Reference state\n \"Reference_position_x\",\n \"Reference_position_y\",\n \"Reference_position_z\",\n \"Reference_orientation_w\",\n \"Reference_orientation_x\",\n \"Reference_orientation_y\",\n \"Reference_orientation_z\",\n \"Reference_v_linear_x\",\n \"Reference_v_linear_y\",\n \"Reference_v_linear_z\",\n \"Reference_v_angular_x\",\n \"Reference_v_angular_y\",\n \"Reference_v_angular_z\",\n # MPC output with GT Postion\n \"Gt_control_command_collective_thrust\",\n \"Gt_control_command_bodyrates_x\",\n \"Gt_control_command_bodyrates_y\",\n \"Gt_control_command_bodyrates_z\",\n # Net output\n \"Net_control_command_collective_thrust\",\n \"Net_control_command_bodyrates_x\",\n \"Net_control_command_bodyrates_y\",\n \"Net_control_command_bodyrates_z\",\n \"Maneuver_type\"]\n\n current_time = datetime.datetime.now().strftime(\"%Y%m%d-%H%M%S\")\n if self.mode == 'iterative':\n root_save_dir = self.config.train_dir\n else:\n root_save_dir = self.config.log_dir\n self.csv_filename = os.path.join(root_save_dir, \"data_\" + current_time + \".csv\")\n self.image_save_dir = os.path.join(root_save_dir, \"img_data_\" + current_time)\n if not os.path.exists(self.image_save_dir):\n os.makedirs(self.image_save_dir)\n with open(self.csv_filename, 'w') as writeFile:\n writer = csv.writer(writeFile)\n writer.writerows([row])\n" }, { "path": "controller_learning/src/ControllerLearning/__init__.py", "content": "" }, { "path": "controller_learning/src/ControllerLearning/models/__init__.py", "content": "" }, { "path": "controller_learning/src/ControllerLearning/models/body_dataset.py", "content": "import numpy as np\nimport glob\nimport os\nimport pandas as pd\nimport tensorflow as tf\nimport fnmatch\nimport cv2\nimport random\nfrom scipy.spatial.transform import Rotation as R\n\n\ndef create_dataset(directory, settings, training=True):\n dataset = SafeDataset(directory, settings, training)\n return dataset\n\n\nclass BodyDataset:\n \"\"\"\n Base Dataset Class\n \"\"\"\n\n def __init__(self, directory, config, training=True):\n self.config = config\n self.directory = directory\n self.training = training\n self.samples = 0\n self.experiments = []\n self.features = []\n self.labels = []\n self.filenames = []\n self.stacked_filenames = [] # Will be used for passing stacked fnames\n img_rootname = 'img_data'\n for root, dirs, files in os.walk(directory, topdown=True, followlinks=True):\n for name in dirs:\n if name.startswith(img_rootname):\n exp_dir = os.path.join(root, name)\n self.experiments.append(os.path.abspath(exp_dir))\n\n self.num_experiments = len(self.experiments)\n self.img_format = 'npy'\n self.data_format = 'csv'\n\n for exp_dir in self.experiments:\n try:\n self._decode_experiment_dir(exp_dir)\n except:\n raise ImportWarning(\"Image reading in {} failed\".format(\n exp_dir))\n if self.samples == 0:\n raise IOError(\"Did not find any file in the dataset folder\")\n print('Found {} images belonging to {} experiments:'.format(\n self.samples, self.num_experiments))\n\n def _recursive_list(self, subpath, fmt='jpg'):\n return fnmatch.filter(os.listdir(subpath), '*.{}'.format(fmt))\n\n def build_dataset(self):\n self._build_dataset()\n\n def _build_dataset(self):\n raise NotImplementedError\n\n def _decode_experiment_dir(self, directory):\n raise NotImplementedError\n\n\nclass SafeDataset(BodyDataset):\n def __init__(self, directory, config, training=True):\n super(SafeDataset, self).__init__(directory, config, training)\n self.build_dataset()\n\n def _decode_experiment_dir(self, dir_subpath):\n base_path = os.path.basename(dir_subpath)\n parent_dict = os.path.dirname(dir_subpath)\n data_name = 'data' + base_path[8:] + \".csv\"\n data_name = os.path.join(parent_dict, data_name)\n assert os.path.isfile(data_name), \"Not Found data file\"\n df = pd.read_csv(data_name, delimiter=',')\n num_files = df.shape[0]\n num_images = len(self._recursive_list(dir_subpath, fmt=self.img_format))\n assert num_files == num_images, \"Number of features and images does not match\"\n\n features_imu = [# VIO Estimate\n \"Orientation_x\",\n \"Orientation_y\",\n \"Orientation_z\",\n \"Orientation_w\",\n \"V_linear_x\",\n \"V_linear_y\",\n \"V_linear_z\",\n \"V_angular_x\",\n \"V_angular_y\",\n \"V_angular_z\"]\n\n features = [# Reference state\n \"Reference_orientation_x\",\n \"Reference_orientation_y\",\n \"Reference_orientation_z\",\n \"Reference_orientation_w\",\n \"Reference_v_linear_x\",\n \"Reference_v_linear_y\",\n \"Reference_v_linear_z\",\n \"Reference_v_angular_x\",\n \"Reference_v_angular_y\",\n \"Reference_v_angular_z\"]\n\n # Preprocessing: we select the good rollouts (no crash in training data)\n rollout_fts = [\"Rollout_idx\"]\n rollout_fts_v = df[rollout_fts].values\n position_gt = [\"gt_Position_x\",\n \"gt_Position_y\",\n \"gt_Position_z\"]\n position_gt_v = df[position_gt].values\n position_ref = [\"Reference_position_x\",\n \"Reference_position_y\",\n \"Reference_position_z\"]\n position_ref_v = df[position_ref].values\n\n good_rollouts = []\n\n if rollout_fts_v.shape[0] == 0:\n return\n\n for r in np.arange(1,np.max(rollout_fts_v)+1):\n rollout_positions = rollout_fts_v == r\n roll_gt = position_gt_v[np.squeeze(rollout_positions),:]\n roll_ref = position_ref_v[np.squeeze(rollout_positions),:]\n if roll_gt.shape[0] == 0:\n continue\n assert roll_ref.shape == roll_gt.shape\n error = np.mean(np.linalg.norm(roll_gt - roll_ref, axis=1))\n if error < self.config.max_allowed_error:\n good_rollouts.append(r)\n\n if self.config.use_imu:\n features = [\"Rollout_idx\"] + features_imu + features\n else:\n features = [\"Rollout_idx\"] + features\n\n labels = [\"Gt_control_command_collective_thrust\",\n \"Gt_control_command_bodyrates_x\",\n \"Gt_control_command_bodyrates_y\",\n \"Gt_control_command_bodyrates_z\"]\n\n features_v = df[features].values\n labels_v = df[labels].values\n\n for frame_number in range(num_files):\n is_valid = False\n img_fname = os.path.join(dir_subpath,\n \"{:08d}.{}\".format(frame_number, self.img_format))\n if os.path.isfile(img_fname) and (rollout_fts_v[frame_number] in good_rollouts):\n is_valid = True\n if is_valid:\n self.features.append(self.preprocess_fts(features_v[frame_number]))\n self.labels.append(labels_v[frame_number])\n if self.config.use_fts_tracks:\n self.filenames.append(img_fname)\n self.samples += 1\n\n def preprocess_fts(self, fts):\n \"\"\"\n Converts rotations from quadrans to rotation matrix.\n Fts have the following indexing.\n rollout_idx, qx,qy,qz,qw, vx, vy, vz, ax, ay, az, rqx, rqy, rqz, rqw, ...\n \"\"\"\n fts = fts.tolist()\n ref_rot = R.from_quat(fts[11:15]).as_matrix().reshape((9,)).tolist()\n if self.config.use_imu:\n odom_rot = R.from_quat(fts[1:5]).as_matrix().reshape((9,)).tolist()\n processed_fts = [fts[0]] + odom_rot + fts[5:11] + ref_rot + fts[15:]\n else:\n processed_fts = [fts[0]] + ref_rot + fts[5:11]\n return np.array(processed_fts)\n\n def add_missing_fts(self, features_dict):\n processed_dict = features_dict\n # Could be both positive or negative\n missing_fts = self.config.min_number_fts - len(features_dict.keys())\n if missing_fts > 0:\n # Features are missing\n if missing_fts != self.config.min_number_fts:\n # There is something, we can sample\n new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts))\n for j in range(missing_fts):\n processed_dict[-j-1] = features_dict[new_features_keys[j]]\n else:\n # Zero features, this is a transient\n for j in range(missing_fts):\n processed_dict[-j-1] = np.zeros((5,))\n elif missing_fts < 0:\n # There are more features than we need, so sample\n del_features_keys = random.sample(features_dict.keys(), int(-missing_fts))\n for k in del_features_keys:\n del processed_dict[k]\n return processed_dict\n\n def load_fts_sequence(self, sample_num):\n fts_seq = []\n sample_num_np = sample_num.numpy()\n filenames_num = self.stacked_filenames[sample_num_np]\n for idx in range(self.config.seq_len):\n fname_idx = filenames_num[idx]\n if fname_idx < 0:\n fts = {}\n else:\n fname = self.filenames[fname_idx]\n fts = np.load(fname, allow_pickle=True).item()\n fts_seq.append(fts)\n # Reverse list to have it ordered in time (t-seq_len, ..., t)\n fts_seq = reversed(fts_seq)\n # Crop to the required lenght\n fts_seq = [self.add_missing_fts(ft) for ft in fts_seq]\n # Stack\n features_input = np.stack([np.stack([v for v in fts_seq[j].values()]) \\\n for j in range(self.config.seq_len)])\n return features_input\n\n def _dataset_map(self, sample_num):\n # First is rollout idx\n label = tf.gather(self.labels, sample_num)\n state_seq = []\n\n # For states is easy: nothing to do.\n for idx in reversed(range(self.config.seq_len)):\n state = tf.gather(self.features, sample_num - idx)[1:]\n state_seq.append(state)\n\n state_seq = tf.stack(state_seq)\n\n # For images, take care they do not overlap\n if self.config.use_fts_tracks:\n fts_seq = tf.py_function(func=self.load_fts_sequence,\n inp=[sample_num],\n Tout=tf.float32)\n\n return (state_seq, fts_seq), label\n else:\n return state_seq, label\n\n def check_equal_dict(self, d1, d2):\n for k_1, v_1 in d1.items():\n try:\n v_2 = d2[k_1]\n except:\n return False\n if not np.array_equal(v_1,v_2):\n return False\n return True\n\n def _preprocess_fnames(self):\n # Append filenames up to seq_len for fast loading.\n # A bit ugly and inefficent, can be improved\n self.last_init_fts = None\n for k in range(len(self.filenames)):\n if k % 3000 == 0:\n print(\"Built {:.2f}% of the dataset\".format(\n k/len(self.filenames)*100), end='\\r')\n # Check if you can copy the things before\n kth_fts = self.filenames[k]\n kth_fts = np.load(kth_fts, allow_pickle=True).item()\n if k > 0:\n if self.check_equal_dict(self.last_init_fts, kth_fts):\n self.stacked_filenames.append(self.stacked_filenames[-1])\n continue\n # This is the lastest observed feature track different from others\n self.last_init_fts = kth_fts\n idx = 0\n rollout_idxes = []\n fname_seq = []\n fts_seq = []\n while len(fts_seq) < self.config.seq_len:\n if k - idx < 0:\n #this is transient, can only append zeros\n fname_seq.append(-1)\n fts_seq.append(0.)\n continue\n current_idx = k - idx\n rollout_idx = self.features[current_idx][0]\n if idx == 0:\n fname_seq.append(current_idx)\n fts_seq.append(kth_fts)\n rollout_idxes.append(rollout_idx)\n else:\n if rollout_idx != rollout_idxes[-1]:\n # it is a transient! Can only append zeros.\n fname_seq.append(-1)\n fts_seq.append(0.)\n else:\n # Check the features are different\n fname = self.filenames[current_idx]\n fts = np.load(fname, allow_pickle=True).item()\n if not self.check_equal_dict(fts, fts_seq[-1]):\n # Objects are not equal, can append\n fname_seq.append(current_idx)\n fts_seq.append(fts)\n rollout_idxes.append(rollout_idx)\n idx += 1\n assert len(fts_seq) == len(fname_seq)\n self.stacked_filenames.append(fname_seq)\n # EndFor\n assert len(self.filenames) == len(self.stacked_filenames)\n\n def _build_dataset(self):\n # Need to take care that rollout_idxs are consistent\n self.features = np.stack(self.features)\n self.features = self.features.astype(np.float32)\n self.labels = np.stack(self.labels)\n self.labels = self.labels.astype(np.float32)\n last_fname_numbers = []\n if self.config.use_fts_tracks:\n self._preprocess_fnames()\n # Preprocess filenames to assess consistency of experiment\n for idx in range(self.config.seq_len-1, self.samples):\n if self.features[idx,0] == self.features[idx-self.config.seq_len+1,0]:\n last_fname_numbers.append(np.int32(idx))\n\n if self.training:\n np.random.shuffle(last_fname_numbers)\n\n # Form training batches\n dataset = tf.data.Dataset.from_tensor_slices(last_fname_numbers)\n if self.training:\n dataset = dataset.shuffle(buffer_size=len(last_fname_numbers))\n dataset = dataset.map(self._dataset_map,\n num_parallel_calls=10 if self.training else 1)\n dataset = dataset.batch(self.config.batch_size,\n drop_remainder=not self.training)\n dataset = dataset.prefetch(buffer_size=10*self.config.batch_size)\n self.batched_dataset = dataset\n" }, { "path": "controller_learning/src/ControllerLearning/models/bodyrate_learner.py", "content": "import datetime\nimport os\n\nimport numpy as np\nimport tensorflow as tf\nfrom tqdm import tqdm\n\n# Required for catkin import strategy\ntry:\n from .nets import create_network\n from .body_dataset import create_dataset\nexcept:\n from nets import create_network\n from body_dataset import create_dataset\n\nos.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'\n\n\nclass BodyrateLearner(object):\n def __init__(self, settings):\n self.config = settings\n physical_devices = tf.config.experimental.list_physical_devices('GPU')\n if len(physical_devices) > 0:\n tf.config.experimental.set_memory_growth(physical_devices[0], True)\n\n self.min_val_loss = tf.Variable(np.inf,\n name='min_val_loss',\n trainable=False)\n\n self.network = create_network(self.config)\n self.loss = tf.keras.losses.MeanSquaredError()\n self.optimizer = tf.keras.optimizers.Adam(learning_rate=1e-4, clipvalue=.2)\n\n self.train_loss = tf.keras.metrics.Mean(name='train_loss')\n self.val_loss = tf.keras.metrics.Mean(name='validation_loss')\n\n self.global_epoch = tf.Variable(0)\n\n self.ckpt = tf.train.Checkpoint(step=self.global_epoch,\n optimizer=self.optimizer,\n net=self.network)\n\n if self.config.resume_training:\n if self.ckpt.restore(self.config.resume_ckpt_file):\n print(\"------------------------------------------\")\n print(\"Restored from {}\".format(self.config.resume_ckpt_file))\n print(\"------------------------------------------\")\n return\n\n print(\"------------------------------------------\")\n print(\"Initializing from scratch.\")\n print(\"------------------------------------------\")\n\n @tf.function\n def train_step(self, inputs, labels):\n with tf.GradientTape() as tape:\n predictions = self.network(inputs)\n loss = self.loss(labels, predictions)\n gradients = tape.gradient(loss, self.network.trainable_variables)\n self.optimizer.apply_gradients(zip(gradients, self.network.trainable_variables))\n self.train_loss.update_state(loss)\n return gradients\n\n @tf.function\n def val_step(self, inputs, labels):\n predictions = self.network(inputs)\n loss = self.loss(labels, predictions)\n self.val_loss.update_state(loss)\n\n def adapt_input_data(self, features):\n if self.config.use_fts_tracks:\n inputs = {\"fts\": features[1],\n \"state\": features[0]}\n else:\n inputs = {\"state\": features}\n return inputs\n\n def write_train_summaries(self, features, gradients):\n with self.summary_writer.as_default():\n tf.summary.scalar('Train Loss', self.train_loss.result(),\n step=self.optimizer.iterations)\n for g, v in zip(gradients, self.network.trainable_variables):\n tf.summary.histogram(v.name, g, step=self.optimizer.iterations)\n\n def train(self):\n print(\"Training Network\")\n if not hasattr(self, 'train_log_dir'):\n # This should be done only once\n self.train_log_dir = os.path.join(self.config.log_dir, 'train')\n self.summary_writer = tf.summary.create_file_writer(self.train_log_dir)\n self.ckpt_manager = tf.train.CheckpointManager(self.ckpt, self.train_log_dir, max_to_keep=10)\n else:\n # We are in dagger mode, so let us reset the best loss\n self.min_val_loss = np.inf\n self.train_loss.reset_states()\n self.val_loss.reset_states()\n\n dataset_train = create_dataset(self.config.train_dir,\n self.config, training=True)\n dataset_val = create_dataset(self.config.val_dir,\n self.config, training=False)\n\n for epoch in range(self.config.max_training_epochs):\n # Train\n for k, (features, label) in enumerate(tqdm(dataset_train.batched_dataset)):\n features = self.adapt_input_data(features)\n gradients = self.train_step(features, label)\n if tf.equal(k % self.config.summary_freq, 0):\n self.write_train_summaries(features, gradients)\n self.train_loss.reset_states()\n # Eval\n for features, label in tqdm(dataset_val.batched_dataset):\n features = self.adapt_input_data(features)\n self.val_step(features, label)\n validation_loss = self.val_loss.result()\n with self.summary_writer.as_default():\n tf.summary.scalar(\"Validation Loss\", validation_loss, step=tf.cast(self.global_epoch, dtype=tf.int64))\n self.val_loss.reset_states()\n\n self.global_epoch = self.global_epoch + 1\n self.ckpt.step.assign_add(1)\n\n print(\"Epoch: {:2d}, Validation Loss: {:.4f}\".format(self.global_epoch, validation_loss))\n\n if validation_loss < self.min_val_loss or ((epoch + 1) % self.config.save_every_n_epochs) == 0:\n if validation_loss < self.min_val_loss:\n self.min_val_loss = validation_loss\n save_path = self.ckpt_manager.save()\n print(\"Saved checkpoint for epoch {}: {}\".format(int(self.ckpt.step), save_path))\n\n # Reset the metrics for the next epoch\n print(\"------------------------------\")\n print(\"Training finished successfully\")\n print(\"------------------------------\")\n\n def test(self):\n print(\"Testing Network\")\n self.train_log_dir = os.path.join(self.config.log_dir, 'test')\n dataset_val = create_dataset(self.config.test_dir,\n self.config, training=False)\n\n for features, label in tqdm(dataset_val.batched_dataset):\n features = self.adapt_input_data(features)\n self.val_step(features, label)\n validation_loss = self.val_loss.result()\n self.val_loss.reset_states()\n\n print(\"Testing Loss: {:.4f}\".format(validation_loss))\n\n @tf.function\n def inference(self, inputs):\n predictions = self.network(inputs)\n return predictions\n" }, { "path": "controller_learning/src/ControllerLearning/models/nets.py", "content": "import tensorflow as tf\nfrom tensorflow.keras import Model\nfrom tensorflow.keras.layers import Dense, Conv2D, LeakyReLU, Conv1D\nfrom tensorflow.keras.layers import Flatten, GlobalAveragePooling2D\n\ntry:\n from .tf_addons_normalizations import InstanceNormalization\nexcept:\n from tf_addons_normalizations import InstanceNormalization\n\ndef create_network(settings):\n net = AggressiveNet(settings)\n return net\n\n\nclass Network(Model):\n def __init__(self):\n super(Network, self).__init__()\n\n def create(self):\n self._create()\n\n def call(self, x):\n return self._internal_call(x)\n\n def _create(self):\n raise NotImplementedError\n\n def _internal_call(self):\n raise NotImplementedError\n\nclass AggressiveNet(Network):\n def __init__(self, config):\n super(AggressiveNet, self).__init__()\n self.config = config\n self._create(input_size=(config.seq_len, config.min_number_fts, 5))\n\n def _create(self, input_size, has_bias=True, learn_affine=True):\n \"\"\"Init.\n Args:\n input_size (float): size of input\n has_bias (bool, optional): Defaults to True. Conv1d bias?\n learn_affine (bool, optional): Defaults to True. InstanceNorm1d affine?\n \"\"\"\n if self.config.use_fts_tracks:\n f = 2.0\n self.pointnet = [Conv2D(int(16 * f), kernel_size=1, strides=1, padding='valid',\n dilation_rate=1, use_bias=has_bias, input_shape=input_size),\n InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),\n LeakyReLU(alpha=1e-2),\n Conv2D(int(32 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,\n use_bias=has_bias),\n InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),\n LeakyReLU(alpha=1e-2),\n Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,\n use_bias=has_bias),\n InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),\n LeakyReLU(alpha=1e-2),\n Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,\n use_bias=has_bias),\n GlobalAveragePooling2D()]\n\n input_size = (self.config.seq_len, int(64*f))\n self.fts_mergenet = [Conv1D(int(64 * f), kernel_size=2, strides=1, padding='same',\n dilation_rate=1, input_shape=input_size),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Flatten(),\n Dense(int(64*f))]\n\n g = 2.0\n self.states_conv = [Conv1D(int(64 * g), kernel_size=2, strides=1, padding='same',\n dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n LeakyReLU(alpha=1e-2),\n Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),\n Flatten(),\n Dense(int(64*g))]\n\n self.control_module = [Dense(64*g),\n LeakyReLU(alpha=1e-2),\n Dense(32*g),\n LeakyReLU(alpha=1e-2),\n Dense(16*g),\n LeakyReLU(alpha=1e-2),\n Dense(4)]\n\n def _pointnet_branch(self, single_t_features):\n x = tf.expand_dims(single_t_features, axis=1)\n for f in self.pointnet:\n x = f(x)\n return x\n\n def _features_branch(self, input_features):\n preprocessed_fts = tf.map_fn(self._pointnet_branch,\n elems=input_features,\n parallel_iterations=self.config.seq_len) # (seq_len, batch_size, 64)\n preprocessed_fts = tf.transpose(preprocessed_fts, (1,0,2)) # (batch_size, seq_len, 64)\n x = preprocessed_fts\n for f in self.fts_mergenet:\n x = f(x)\n return x\n\n def _states_branch(self, embeddings):\n x = embeddings\n for f in self.states_conv:\n x = f(x)\n return x\n\n def _control_branch(self, embeddings):\n x = embeddings\n for f in self.control_module:\n x = f(x)\n return x\n\n def _internal_call(self, inputs):\n states = inputs['state']\n if self.config.use_fts_tracks:\n fts_stack = inputs['fts'] # (batch_size, seq_len, min_numb_features, 5)\n fts_stack = tf.transpose(fts_stack, (1,0,2,3)) # (seq_len, batch_size, min_numb_features, 5)\n # Execute PointNet Part\n fts_embeddings = self._features_branch(fts_stack)\n states_embeddings = self._states_branch(states)\n if self.config.use_fts_tracks:\n total_embeddings = tf.concat((fts_embeddings, states_embeddings), axis=1)\n else:\n total_embeddings = states_embeddings\n output = self._control_branch(total_embeddings)\n return output\n" }, { "path": "controller_learning/src/ControllerLearning/models/tf_addons_normalizations.py", "content": "# Copyright 2019 The TensorFlow Authors. All Rights Reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"License\");\n# you may not use this file except in compliance with the License.\n# You may obtain a copy of the License at\n#\n# http://www.apache.org/licenses/LICENSE-2.0\n#\n# Unless required by applicable law or agreed to in writing, software\n# distributed under the License is distributed on an \"AS IS\" BASIS,\n# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n# See the License for the specific language governing permissions and\n# limitations under the License.\n\n# Orginal implementation from keras_contrib/layer/normalization\n# The Authors of the drone_acrobatics repository do not own any right on this file.\n# If you have problems with this file, install the tf_addons repository directly.\n# =============================================================================\nfrom __future__ import absolute_import\nfrom __future__ import division\nfrom __future__ import print_function\n\nimport tensorflow as tf\n\nclass GroupNormalization(tf.keras.layers.Layer):\n \"\"\"Group normalization layer.\n\n Group Normalization divides the channels into groups and computes\n within each group the mean and variance for normalization.\n Empirically, its accuracy is more stable than batch norm in a wide\n range of small batch sizes, if learning rate is adjusted linearly\n with batch sizes.\n\n Relation to Layer Normalization:\n If the number of groups is set to 1, then this operation becomes identical\n to Layer Normalization.\n\n Relation to Instance Normalization:\n If the number of groups is set to the\n input dimension (number of groups is equal\n to number of channels), then this operation becomes\n identical to Instance Normalization.\n\n Arguments\n groups: Integer, the number of groups for Group Normalization.\n Can be in the range [1, N] where N is the input dimension.\n The input dimension must be divisible by the number of groups.\n axis: Integer, the axis that should be normalized.\n epsilon: Small float added to variance to avoid dividing by zero.\n center: If True, add offset of `beta` to normalized tensor.\n If False, `beta` is ignored.\n scale: If True, multiply by `gamma`.\n If False, `gamma` is not used.\n beta_initializer: Initializer for the beta weight.\n gamma_initializer: Initializer for the gamma weight.\n beta_regularizer: Optional regularizer for the beta weight.\n gamma_regularizer: Optional regularizer for the gamma weight.\n beta_constraint: Optional constraint for the beta weight.\n gamma_constraint: Optional constraint for the gamma weight.\n\n Input shape\n Arbitrary. Use the keyword argument `input_shape`\n (tuple of integers, does not include the samples axis)\n when using this layer as the first layer in a model.\n\n Output shape\n Same shape as input.\n References\n - [Group Normalization](https://arxiv.org/abs/1803.08494)\n \"\"\"\n\n def __init__(self,\n groups=2,\n axis=-1,\n epsilon=1e-3,\n center=True,\n scale=True,\n beta_initializer='zeros',\n gamma_initializer='ones',\n beta_regularizer=None,\n gamma_regularizer=None,\n beta_constraint=None,\n gamma_constraint=None,\n **kwargs):\n super(GroupNormalization, self).__init__(**kwargs)\n self.supports_masking = True\n self.groups = groups\n self.axis = axis\n self.epsilon = epsilon\n self.center = center\n self.scale = scale\n self.beta_initializer = tf.keras.initializers.get(beta_initializer)\n self.gamma_initializer = tf.keras.initializers.get(gamma_initializer)\n self.beta_regularizer = tf.keras.regularizers.get(beta_regularizer)\n self.gamma_regularizer = tf.keras.regularizers.get(gamma_regularizer)\n self.beta_constraint = tf.keras.constraints.get(beta_constraint)\n self.gamma_constraint = tf.keras.constraints.get(gamma_constraint)\n self._check_axis()\n\n def build(self, input_shape):\n\n self._check_if_input_shape_is_none(input_shape)\n self._set_number_of_groups_for_instance_norm(input_shape)\n self._check_size_of_dimensions(input_shape)\n self._create_input_spec(input_shape)\n\n self._add_gamma_weight(input_shape)\n self._add_beta_weight(input_shape)\n self.built = True\n super(GroupNormalization, self).build(input_shape)\n\n def call(self, inputs):\n\n input_shape = tf.keras.backend.int_shape(inputs)\n tensor_input_shape = tf.shape(inputs)\n\n reshaped_inputs, group_shape = self._reshape_into_groups(\n inputs, input_shape, tensor_input_shape)\n\n normalized_inputs = self._apply_normalization(reshaped_inputs,\n input_shape)\n\n outputs = tf.reshape(normalized_inputs, tensor_input_shape)\n\n return outputs\n\n def get_config(self):\n config = {\n 'groups':\n self.groups,\n 'axis':\n self.axis,\n 'epsilon':\n self.epsilon,\n 'center':\n self.center,\n 'scale':\n self.scale,\n 'beta_initializer':\n tf.keras.initializers.serialize(self.beta_initializer),\n 'gamma_initializer':\n tf.keras.initializers.serialize(self.gamma_initializer),\n 'beta_regularizer':\n tf.keras.regularizers.serialize(self.beta_regularizer),\n 'gamma_regularizer':\n tf.keras.regularizers.serialize(self.gamma_regularizer),\n 'beta_constraint':\n tf.keras.constraints.serialize(self.beta_constraint),\n 'gamma_constraint':\n tf.keras.constraints.serialize(self.gamma_constraint)\n }\n base_config = super(GroupNormalization, self).get_config()\n return dict(list(base_config.items()) + list(config.items()))\n\n def compute_output_shape(self, input_shape):\n return input_shape\n\n def _reshape_into_groups(self, inputs, input_shape, tensor_input_shape):\n\n group_shape = [tensor_input_shape[i] for i in range(len(input_shape))]\n group_shape[self.axis] = input_shape[self.axis] // self.groups\n group_shape.insert(1, self.groups)\n group_shape = tf.stack(group_shape)\n reshaped_inputs = tf.reshape(inputs, group_shape)\n return reshaped_inputs, group_shape\n\n def _apply_normalization(self, reshaped_inputs, input_shape):\n\n group_shape = tf.keras.backend.int_shape(reshaped_inputs)\n group_reduction_axes = list(range(len(group_shape)))\n # Remember the ordering of the tensor is [batch, group , steps]. Jump\n # the first 2 to calculate the variance and the mean\n mean, variance = tf.nn.moments(\n reshaped_inputs, group_reduction_axes[2:], keepdims=True)\n\n gamma, beta = self._get_reshaped_weights(input_shape)\n normalized_inputs = tf.nn.batch_normalization(\n reshaped_inputs,\n mean=mean,\n variance=variance,\n scale=gamma,\n offset=beta,\n variance_epsilon=self.epsilon)\n return normalized_inputs\n\n def _get_reshaped_weights(self, input_shape):\n broadcast_shape = self._create_broadcast_shape(input_shape)\n gamma = None\n beta = None\n if self.scale:\n gamma = tf.reshape(self.gamma, broadcast_shape)\n\n if self.center:\n beta = tf.reshape(self.beta, broadcast_shape)\n return gamma, beta\n\n def _check_if_input_shape_is_none(self, input_shape):\n dim = input_shape[self.axis]\n if dim is None:\n raise ValueError('Axis ' + str(self.axis) + ' of '\n 'input tensor should have a defined dimension '\n 'but the layer received an input with shape ' +\n str(input_shape) + '.')\n\n def _set_number_of_groups_for_instance_norm(self, input_shape):\n dim = input_shape[self.axis]\n\n if self.groups == -1:\n self.groups = dim\n\n def _check_size_of_dimensions(self, input_shape):\n\n dim = input_shape[self.axis]\n if dim < self.groups:\n raise ValueError(\n 'Number of groups (' + str(self.groups) + ') cannot be '\n 'more than the number of channels (' + str(dim) + ').')\n\n if dim % self.groups != 0:\n raise ValueError(\n 'Number of groups (' + str(self.groups) + ') must be a '\n 'multiple of the number of channels (' + str(dim) + ').')\n\n def _check_axis(self):\n\n if self.axis == 0:\n raise ValueError(\n \"You are trying to normalize your batch axis. Do you want to \"\n \"use tf.layer.batch_normalization instead\")\n\n def _create_input_spec(self, input_shape):\n\n dim = input_shape[self.axis]\n self.input_spec = tf.keras.layers.InputSpec(\n ndim=len(input_shape), axes={self.axis: dim})\n\n def _add_gamma_weight(self, input_shape):\n\n dim = input_shape[self.axis]\n shape = (dim,)\n\n if self.scale:\n self.gamma = self.add_weight(\n shape=shape,\n name='gamma',\n initializer=self.gamma_initializer,\n regularizer=self.gamma_regularizer,\n constraint=self.gamma_constraint)\n else:\n self.gamma = None\n\n def _add_beta_weight(self, input_shape):\n\n dim = input_shape[self.axis]\n shape = (dim,)\n\n if self.center:\n self.beta = self.add_weight(\n shape=shape,\n name='beta',\n initializer=self.beta_initializer,\n regularizer=self.beta_regularizer,\n constraint=self.beta_constraint)\n else:\n self.beta = None\n\n def _create_broadcast_shape(self, input_shape):\n broadcast_shape = [1] * len(input_shape)\n broadcast_shape[self.axis] = input_shape[self.axis] // self.groups\n broadcast_shape.insert(1, self.groups)\n return broadcast_shape\n\n\n# @keras_utils.register_keras_custom_object\nclass InstanceNormalization(GroupNormalization):\n \"\"\"Instance normalization layer.\n\n Instance Normalization is an specific case of ```GroupNormalization```since\n it normalizes all features of one channel. The Groupsize is equal to the\n channel size. Empirically, its accuracy is more stable than batch norm in a\n wide range of small batch sizes, if learning rate is adjusted linearly\n with batch sizes.\n\n Arguments\n axis: Integer, the axis that should be normalized.\n epsilon: Small float added to variance to avoid dividing by zero.\n center: If True, add offset of `beta` to normalized tensor.\n If False, `beta` is ignored.\n scale: If True, multiply by `gamma`.\n If False, `gamma` is not used.\n beta_initializer: Initializer for the beta weight.\n gamma_initializer: Initializer for the gamma weight.\n beta_regularizer: Optional regularizer for the beta weight.\n gamma_regularizer: Optional regularizer for the gamma weight.\n beta_constraint: Optional constraint for the beta weight.\n gamma_constraint: Optional constraint for the gamma weight.\n\n Input shape\n Arbitrary. Use the keyword argument `input_shape`\n (tuple of integers, does not include the samples axis)\n when using this layer as the first layer in a model.\n\n Output shape\n Same shape as input.\n\n References\n - [Instance Normalization: The Missing Ingredient for Fast Stylization]\n (https://arxiv.org/abs/1607.08022)\n \"\"\"\n\n def __init__(self, **kwargs):\n kwargs[\"groups\"] = -1\n super(InstanceNormalization, self).__init__(**kwargs)\n" }, { "path": "controller_learning/test_trajectories.py", "content": "import argparse\n\nfrom common import setup_sim, update_mpc_params\nfrom config.settings import create_settings\nfrom iterative_learning_trajectories import Trainer\n\n\ndef main():\n parser = argparse.ArgumentParser(description='Evaluate Trajectory tracker.')\n parser.add_argument('--settings_file', help='Path to settings yaml', required=True)\n\n args = parser.parse_args()\n settings_filepath = args.settings_file\n settings = create_settings(settings_filepath, mode='test')\n update_mpc_params()\n setup_sim()\n trainer = Trainer(settings)\n trainer.perform_testing()\n\n\nif __name__ == \"__main__\":\n main()\n" }, { "path": "controller_learning/train.py", "content": "#!/usr/bin/env python3\n\nimport argparse\nimport os\nimport sys\nsys.path.append(\"./src/ControllerLearning/models/\")\nimport time\n\n#import rospy\nfrom bodyrate_learner import BodyrateLearner\n\nfrom config.settings import create_settings\n\n\ndef main():\n parser = argparse.ArgumentParser(description='Train Network')\n parser.add_argument('--settings_file', help='Path to settings yaml', required=True)\n\n args = parser.parse_args()\n settings_filepath = args.settings_file\n settings = create_settings(settings_filepath, mode='train')\n\n learner = BodyrateLearner(settings=settings)\n learner.train()\n\n\nif __name__ == \"__main__\":\n main()\n" }, { "path": "controller_learning/train_logs/20191015-164750/train/checkpoint", "content": "model_checkpoint_path: \"ckpt-80\"\nall_model_checkpoint_paths: \"ckpt-78\"\nall_model_checkpoint_paths: \"ckpt-79\"\nall_model_checkpoint_paths: \"ckpt-80\"\nall_model_checkpoint_timestamps: 1571151095.56399\nall_model_checkpoint_timestamps: 1571151096.0123613\nall_model_checkpoint_timestamps: 1571151096.455976\nlast_preserved_timestamp: 1571150869.450073\n" }, { "path": "controller_learning/train_logs/20191022-082718/train/checkpoint", "content": "model_checkpoint_path: \"ckpt-1080\"\nall_model_checkpoint_paths: \"ckpt-1078\"\nall_model_checkpoint_paths: \"ckpt-1079\"\nall_model_checkpoint_paths: \"ckpt-1080\"\nall_model_checkpoint_timestamps: 1571726594.0011396\nall_model_checkpoint_timestamps: 1571726594.9023852\nall_model_checkpoint_timestamps: 1571726595.8179815\nlast_preserved_timestamp: 1571725637.457743\n" }, { "path": "controller_learning/train_logs/20191022-095948/train/checkpoint", "content": "model_checkpoint_path: \"ckpt-1000\"\nall_model_checkpoint_paths: \"ckpt-998\"\nall_model_checkpoint_paths: \"ckpt-999\"\nall_model_checkpoint_paths: \"ckpt-1000\"\nall_model_checkpoint_timestamps: 1571733868.2468255\nall_model_checkpoint_timestamps: 1571733870.8290527\nall_model_checkpoint_timestamps: 1571733873.409515\nlast_preserved_timestamp: 1571731187.6806133\n" }, { "path": "custom_rotors_interface/.gitignore", "content": "cmake-build-debug/\n" }, { "path": "custom_rotors_interface/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(custom_rotors_interface)\n\n# Add plain cmake packages\nfind_package(catkin_simple REQUIRED)\ncatkin_simple(ALL_DEPS_REQUIRED)\n\ncs_add_library(custom_rotors_interface src/custom_rotors_interface.cpp)\n\ncs_add_executable(custom_rotors_interface_node src/custom_rotors_interface_node.cpp)\ntarget_link_libraries(custom_rotors_interface_node custom_rotors_interface)\n\ncs_install()\ncs_export()\n\n# CMake Indexing\nFILE(GLOB_RECURSE LibFiles \"include/*\")\nadd_custom_target(headers SOURCES ${LibFiles})\nFILE(GLOB_RECURSE ProtoFiles \"proto/*\")\nadd_custom_target(protos SOURCES ${ProtoFiles})\n" }, { "path": "custom_rotors_interface/include/custom_rotors_interface/custom_rotors_interface.h", "content": "#pragma once\n\n#include \n#include \n\n#include \n#include \n#include \n#include \"mav_msgs/Actuators.h\"\n#include \"nav_msgs/Odometry.h\"\n#include \"quadrotor_msgs/AutopilotFeedback.h\"\n#include \"pose_utils/pose.h\"\n#include \"std_msgs/Bool.h\"\n\nnamespace custom_rotors_interface {\n\nstruct TorquesAndThrust {\n Eigen::Vector3d body_torques;\n double collective_thrust;\n};\n\nclass CustomRotorsInterface {\n public:\n CustomRotorsInterface(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);\n\n CustomRotorsInterface()\n : CustomRotorsInterface(ros::NodeHandle(), ros::NodeHandle(\"~\")) {}\n\n private:\n void armCallback(const std_msgs::BoolConstPtr& msg);\n\n void controlCommandCallback(\n const quadrotor_msgs::ControlCommandConstPtr& msg);\n\n void odometryCallback(const nav_msgs::OdometryConstPtr& msg);\n\n void autopilotCallback(const quadrotor_msgs::AutopilotFeedbackConstPtr& msg);\n\n void motorSpeedCallback(const mav_msgs::Actuators::ConstPtr& msg);\n\n void lowLevelControlLoop();\n\n TorquesAndThrust bodyRateControl(\n const quadrotor_common::ControlCommand& rate_cmd,\n const Eigen::Vector3d& body_rate_estimate);\n\n mav_msgs::Actuators mixer(const TorquesAndThrust& torques_and_thrust);\n\n bool loadParameters();\n\n ros::NodeHandle nh_;\n ros::NodeHandle pnh_;\n\n ros::Subscriber arm_sub_;\n ros::Subscriber motor_speed_sub_;\n ros::Subscriber ctrl_cmd_sub_;\n ros::Subscriber imu_sub_;\n ros::Subscriber odometry_sub_;\n ros::Subscriber controller_sub_;\n ros::Subscriber network_sub_;\n ros::Subscriber autopilot_sub_;\n\n ros::Publisher desired_motor_speed_pub_;\n\n ros::WallTime start_time_;\n\n std::string data_dir_;\n\n Eigen::Vector3d prev_v_lin_, prev_v_ang_;\n Eigen::Vector3d incr_v_lin_, incr_v_ang_;\n\n ros::Time last_hover_time_ = ros::Time::now();\n int maneuver_idx_ = 0;\n int action_idx_ = 0;\n bool velocity_estimate_in_world_frame_;\n bool enable_navigation_ = false;\n bool reference_frame_set_ = false;\n bool interface_armed_ = false;\n\n TorquesAndThrust torques_and_thrust_estimate_;\n\n double inertia_x_;\n double inertia_y_;\n double inertia_z_;\n double body_rates_p_xy_;\n double body_rates_d_xy_;\n double body_rates_p_z_;\n double body_rates_d_z_;\n\n // Parameters\n Eigen::Matrix3d inertia_;\n Eigen::MatrixXd K_lqr_;\n double low_level_control_frequency_;\n double roll_pitch_cont_gain_;\n double arm_length_;\n double rotor_drag_coeff_;\n double rotor_thrust_coeff_;\n double mass_;\n double max_rotor_speed_;\n\n quadrotor_msgs::ControlCommand control_command_;\n nav_msgs::Odometry odometry_;\n quadrotor_msgs::AutopilotFeedback autopilot_feedback_;\n double rel_time_;\n};\n\n} // namespace custom_rotors_interface\n" }, { "path": "custom_rotors_interface/package.xml", "content": "\n\n custom_rotors_interface\n 0.0.0\n A package containing utils for simulation\n\n Elia Kaufmann\n GNU GPL\n\n catkin\n catkin_simple\n\n roscpp\n gazebo\n gazebo_msgs\n mav_msgs\n quadrotor_common\n pose_utils\n rotors_gazebo\n rotors_description\n manual_flight_assistant\n rqt_quad_gui\n\n\n" }, { "path": "custom_rotors_interface/parameters/autopilot.yaml", "content": "state_estimate_timeout: 5.0 # [s]\nvelocity_estimate_in_world_frame: false\ncontrol_command_delay: 0.05 # [s]\n\nstart_land_velocity: 0.5 # [m/s]\nstart_land_acceleration: 1 # [m/s^2]\nstart_idle_duration: 2.0 # [s]\nidle_thrust: 2.0 # [m/s]\noptitrack_start_height: 3.0 # [m]\noptitrack_start_land_timeout: 10 # [s]\noptitrack_land_drop_height: 0.3 # [m]\npropeller_ramp_down_timeout: 1.5 # [s]\n\nbreaking_velocity_threshold: 0.2 # [m/s]\nbreaking_timeout: 0.5 # [s]\n\ngo_to_pose_max_velocity: 3.5 # [m/s]\ngo_to_pose_max_normalized_thrust: 15.0 # [m/s^2]\ngo_to_pose_max_roll_pitch_rate: 1.0 # [rad/s]\n\nvelocity_command_input_timeout: 0.1 # [s]\ntau_velocity_command: 0.8 # []\n\nreference_state_input_timeout: 0.1 # [s]\n\nemergency_land_duration: 4 # [s]\nemergency_land_thrust: 9.0 # [m/s^2]\n\ncontrol_command_input_timeout: 1.0 # [s]\nenable_command_feedthrough: true\npredictive_control_lookahead: 2.0 # [s]\n\nmin_control_period: 0.0 # [s]\n" }, { "path": "custom_rotors_interface/parameters/custom_rotors_interface.yaml", "content": "# Vehicle parameters\ninertia_x: 0.007\ninertia_y: 0.007\ninertia_z: 0.012\narm_length: 0.17\nmass: 0.73\n# Motors parameters\nrotor_drag_coeff: 0.016\nrotor_thrust_coeff: 8.54858e-06\nmax_rotor_speed: 838.0\n# Controller\nlow_level_control_frequency: 200.0\nbody_rates_p_xy: 0.1\nbody_rates_d_xy: 0.5\nbody_rates_p_z: 0.03\nbody_rates_d_z: 0.1\nroll_pitch_cont_gain: 6.0\n" }, { "path": "custom_rotors_interface/parameters/manual_flight_assistant.yaml", "content": "joypad_timeout: 0.5\nsbus_timeout: 0.5\njoypad_axes_zero_tolerance: 0.05\nsbus_axes_zero_tolerance: 10\nvmax_xy: 1.5\nvmax_z: 0.7\nrmax_yaw: 1.5\n" }, { "path": "custom_rotors_interface/parameters/position_controller.yaml", "content": "position_controller:\n use_rate_mode: true\n \n kpxy: 10.0\n kdxy: 4.0\n \n kpz: 15.0\n kdz: 6.0\n krp: 12.0\n kyaw: 5.0\n \n pxy_error_max: 0.6\n vxy_error_max: 1.0\n pz_error_max: 0.3\n vz_error_max: 0.75\n yaw_error_max: 0.7\n \n perform_aerodynamics_compensation: false\n \n k_drag_x: 0.0\n k_drag_y: 0.0\n k_drag_z: 0.0\n k_thrust_horz: 0.0\n" }, { "path": "custom_rotors_interface/src/custom_rotors_interface.cpp", "content": "#include \"custom_rotors_interface/custom_rotors_interface.h\"\n\n#include \n#include \"quadrotor_common/geometry_eigen_conversions.h\"\n\nnamespace custom_rotors_interface {\n\nCustomRotorsInterface::CustomRotorsInterface(const ros::NodeHandle& nh,\n const ros::NodeHandle& pnh)\n : nh_(nh), pnh_(pnh) {\n if (!loadParameters()) {\n ROS_ERROR(\"[%s] Could not load parameters.\", pnh_.getNamespace().c_str());\n }\n\n arm_sub_ =\n nh_.subscribe(\"bridge/arm\", 1, &CustomRotorsInterface::armCallback, this);\n ctrl_cmd_sub_ = nh_.subscribe(\"control_command\", 1,\n &CustomRotorsInterface::controlCommandCallback,\n this, ros::TransportHints().tcpNoDelay());\n autopilot_sub_ = nh_.subscribe(\"autopilot/feedback\", 1,\n &CustomRotorsInterface::autopilotCallback,\n this, ros::TransportHints().tcpNoDelay());\n odometry_sub_ =\n nh_.subscribe(\"odometry\", 1, &CustomRotorsInterface::odometryCallback,\n this, ros::TransportHints().tcpNoDelay());\n motor_speed_sub_ = nh_.subscribe(\"motor_speed\", 1,\n &CustomRotorsInterface::motorSpeedCallback,\n this, ros::TransportHints().tcpNoDelay());\n desired_motor_speed_pub_ =\n nh_.advertise(\"command/motor_speed\", 1);\n}\n\nvoid CustomRotorsInterface::armCallback(const std_msgs::BoolConstPtr& msg) {\n if (msg->data) {\n interface_armed_ = true;\n ROS_INFO(\"[%s] Interface armed\", pnh_.getNamespace().c_str());\n } else {\n interface_armed_ = false;\n ROS_INFO(\"[%s] Interface disarmed\", pnh_.getNamespace().c_str());\n }\n}\n\nvoid CustomRotorsInterface::controlCommandCallback(\n const quadrotor_msgs::ControlCommandConstPtr& msg) {\n control_command_ = *msg;\n}\n\nvoid CustomRotorsInterface::autopilotCallback(\n const quadrotor_msgs::AutopilotFeedbackConstPtr& msg) {\n autopilot_feedback_ = *msg;\n}\n\nvoid CustomRotorsInterface::odometryCallback(\n const nav_msgs::OdometryConstPtr& msg) {\n odometry_ = *msg;\n}\n\nvoid CustomRotorsInterface::motorSpeedCallback(\n const mav_msgs::Actuators::ConstPtr& msg) {\n // The hummingbird that we use in our simulations has the following rotor\n // configuration Rotor 0 spins clockwise\n // x\n // 0 ^\n // | |\n // 1--+--3 y <--+\n // |\n // 2\n\n const double f0 = rotor_thrust_coeff_ * pow(msg->angular_velocities[0], 2.0);\n const double f1 = rotor_thrust_coeff_ * pow(msg->angular_velocities[1], 2.0);\n const double f2 = rotor_thrust_coeff_ * pow(msg->angular_velocities[2], 2.0);\n const double f3 = rotor_thrust_coeff_ * pow(msg->angular_velocities[3], 2.0);\n\n torques_and_thrust_estimate_.body_torques.x() = arm_length_ * (f1 - f3);\n torques_and_thrust_estimate_.body_torques.y() = arm_length_ * (f2 - f0);\n torques_and_thrust_estimate_.body_torques.z() =\n rotor_drag_coeff_ * (f0 - f1 + f2 - f3);\n torques_and_thrust_estimate_.collective_thrust = f0 + f1 + f2 + f3;\n lowLevelControlLoop();\n}\n\nvoid CustomRotorsInterface::lowLevelControlLoop() {\n mav_msgs::Actuators desired_motor_speed;\n quadrotor_msgs::ControlCommand control_command = control_command_;\n nav_msgs::Odometry quad_state = odometry_;\n\n if (!interface_armed_ || !control_command.armed) {\n for (int i = 0; i < 4; i++) {\n desired_motor_speed.angular_velocities.push_back(0.0);\n }\n } else {\n if (control_command.control_mode == control_command.BODY_RATES) {\n const quadrotor_common::ControlCommand rate_cmd =\n quadrotor_common::ControlCommand(control_command);\n const TorquesAndThrust torques_and_thrust = bodyRateControl(\n rate_cmd,\n quadrotor_common::geometryToEigen(quad_state.twist.twist.angular));\n desired_motor_speed = mixer(torques_and_thrust);\n } else {\n ROS_ERROR_THROTTLE(1,\n \"[%s] Undefined control mode, will not apply command.\",\n ros::this_node::getName().c_str());\n return;\n }\n }\n\n desired_motor_speed_pub_.publish(desired_motor_speed);\n}\n\nTorquesAndThrust CustomRotorsInterface::bodyRateControl(\n const quadrotor_common::ControlCommand& rate_cmd,\n const Eigen::Vector3d& body_rate_estimate) {\n Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 3);\n Eigen::VectorXd control_error = Eigen::VectorXd::Zero(6);\n // control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate;\n // control_error.segment(3, 3) = rate_cmd.bodyrates.cross(inertia_ *\n // rate_cmd.bodyrates)\n // + inertia_ * rate_cmd.angular_accelerations\n // - torques_and_thrust_estimate_.body_torques;\n control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate;\n control_error.segment(3, 3) =\n rate_cmd.bodyrates.cross(inertia_ * rate_cmd.bodyrates) +\n inertia_ * rate_cmd.angular_accelerations -\n torques_and_thrust_estimate_.body_torques;\n\n TorquesAndThrust torques_and_thrust;\n // torques_and_thrust.body_torques = K_lqr_ * control_error +\n // body_rate_estimate.cross(inertia_ * body_rate_estimate)\n // + inertia_ *\n // rate_cmd.angular_accelerations;\n // new version\n torques_and_thrust.body_torques =\n K_lqr_ * control_error +\n body_rate_estimate.cross(inertia_ * body_rate_estimate) +\n inertia_ * rate_cmd.angular_accelerations;\n torques_and_thrust.collective_thrust = rate_cmd.collective_thrust;\n\n return torques_and_thrust;\n}\n\nmav_msgs::Actuators CustomRotorsInterface::mixer(\n const TorquesAndThrust& torques_and_thrust) {\n // Using Rotor Convention of the Hummingbird\n mav_msgs::Actuators rotor_speed_cmds;\n for (int i = 0; i < 4; i++) {\n rotor_speed_cmds.angular_velocities.push_back(0.0);\n }\n\n // Compute square of single rotor speed commands\n if (torques_and_thrust.collective_thrust < 0.05) {\n return rotor_speed_cmds;\n }\n // Compute the square of the single rotor speeds\n rotor_speed_cmds.angular_velocities[0] =\n ((arm_length_ * torques_and_thrust.body_torques.z() -\n 2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() +\n rotor_drag_coeff_ * arm_length_ * mass_ *\n torques_and_thrust.collective_thrust) /\n (4.0 * rotor_drag_coeff_ * arm_length_)) /\n rotor_thrust_coeff_;\n rotor_speed_cmds.angular_velocities[1] =\n ((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() -\n arm_length_ * torques_and_thrust.body_torques.z() +\n rotor_drag_coeff_ * arm_length_ * mass_ *\n torques_and_thrust.collective_thrust) /\n (4.0 * rotor_drag_coeff_ * arm_length_)) /\n rotor_thrust_coeff_;\n rotor_speed_cmds.angular_velocities[2] =\n ((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() +\n arm_length_ * torques_and_thrust.body_torques.z() +\n rotor_drag_coeff_ * arm_length_ * mass_ *\n torques_and_thrust.collective_thrust) /\n (4.0 * rotor_drag_coeff_ * arm_length_)) /\n rotor_thrust_coeff_;\n rotor_speed_cmds.angular_velocities[3] =\n (-(2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() +\n arm_length_ * torques_and_thrust.body_torques.z() -\n rotor_drag_coeff_ * arm_length_ * mass_ *\n torques_and_thrust.collective_thrust) /\n (4.0 * rotor_drag_coeff_ * arm_length_)) /\n rotor_thrust_coeff_;\n\n // Apply limits and take square root\n for (int i = 0; i < 4; i++) {\n quadrotor_common::limit(&rotor_speed_cmds.angular_velocities[i], 0.0,\n pow(max_rotor_speed_, 2.0));\n rotor_speed_cmds.angular_velocities[i] =\n sqrt(rotor_speed_cmds.angular_velocities[i]);\n }\n\n rotor_speed_cmds.header.stamp = ros::Time::now();\n return rotor_speed_cmds;\n}\n\nbool CustomRotorsInterface::loadParameters() {\n bool check = true;\n\n check &= pnh_.getParam(\"inertia_x\", inertia_x_);\n check &= pnh_.getParam(\"inertia_y\", inertia_y_);\n check &= pnh_.getParam(\"inertia_z\", inertia_z_);\n\n inertia_ = Eigen::Matrix3d::Zero();\n inertia_(0, 0) = inertia_x_;\n inertia_(1, 1) = inertia_y_;\n inertia_(2, 2) = inertia_z_;\n\n check &= pnh_.getParam(\"body_rates_p_xy\", body_rates_p_xy_);\n check &= pnh_.getParam(\"body_rates_d_xy\", body_rates_d_xy_);\n check &= pnh_.getParam(\"body_rates_p_z\", body_rates_p_z_);\n check &= pnh_.getParam(\"body_rates_d_z\", body_rates_d_z_);\n\n // nothing to do with lqr, just a PD controller\n K_lqr_ = Eigen::MatrixXd::Zero(3, 6);\n K_lqr_(0, 0) = body_rates_p_xy_;\n K_lqr_(1, 1) = body_rates_p_xy_;\n K_lqr_(2, 2) = body_rates_p_z_;\n K_lqr_(0, 3) = body_rates_d_xy_;\n K_lqr_(1, 4) = body_rates_d_xy_;\n K_lqr_(2, 5) = body_rates_d_z_;\n\n check &= pnh_.getParam(\"roll_pitch_cont_gain\", roll_pitch_cont_gain_);\n check &= pnh_.getParam(\"arm_length\", arm_length_);\n check &= pnh_.getParam(\"rotor_drag_coeff\", rotor_drag_coeff_);\n check &= pnh_.getParam(\"rotor_thrust_coeff\", rotor_thrust_coeff_);\n check &= pnh_.getParam(\"mass\", mass_);\n check &= pnh_.getParam(\"max_rotor_speed\", max_rotor_speed_);\n\n return check;\n}\n\n} // namespace custom_rotors_interface\n" }, { "path": "custom_rotors_interface/src/custom_rotors_interface_node.cpp", "content": "#include \n#include \n\nint main(int argc, char **argv) {\n ros::init(argc, argv, \"custom_rotors_interface\");\n\n custom_rotors_interface::CustomRotorsInterface custom_rotors_interface;\n\n ros::spin();\n return 0;\n}\n" }, { "path": "dependencies.yaml", "content": "repositories:\n catkin_boost_python_buildtool:\n type: git\n url: git@github.com:ethz-asl/catkin_boost_python_buildtool.git\n version: master\n catkin_simple:\n type: git\n url: git@github.com:catkin/catkin_simple.git\n version: master\n eigen_catkin:\n type: git\n url: git@github.com:ethz-asl/eigen_catkin.git\n version: master\n eigen_checks:\n type: git\n url: git@github.com:ethz-asl/eigen_checks.git\n version: master\n gflags_catkin:\n type: git\n url: git@github.com:ethz-asl/gflags_catkin.git\n version: master\n glog_catkin:\n type: git\n url: git@github.com:ethz-asl/glog_catkin.git\n version: master\n minkindr:\n type: git\n url: git@github.com:ethz-asl/minkindr.git\n version: master\n minkindr_ros:\n type: git\n url: git@github.com:ethz-asl/minkindr_ros.git\n version: master\n numpy_eigen:\n type: git\n url: git@github.com:ethz-asl/numpy_eigen.git\n version: master\n pose_utils:\n type: git\n url: git@github.com:antonilo/pose_utils.git\n version: master\n minimum_jerk_trajectories:\n type: git\n url: git@github.com:uzh-rpg/minimum_jerk_trajectories.git\n version: master\n octomap:\n type: git\n url: git@github.com:OctoMap/octomap.git\n version: master\n octomap_msgs:\n type: git\n url: git@github.com:OctoMap/octomap_msgs.git\n version: master\n octomap_ros:\n type: git\n url: git@github.com:OctoMap/octomap_ros.git\n version: melodic-devel\n rpg_mpc:\n type: git\n url: git@github.com:uzh-rpg/rpg_mpc.git\n version: master\n rotors_simulator:\n type: git\n url: git@github.com:ethz-asl/rotors_simulator.git\n version: master\n rpg_quadrotor_control:\n type: git\n url: git@github.com:uzh-rpg/rpg_quadrotor_control.git\n version: master\n rpg_quadrotor_common:\n type: git\n url: git@github.com:uzh-rpg/rpg_quadrotor_common.git\n version: master\n mav_comm:\n type: git\n url: git@github.com:ethz-asl/mav_comm.git\n version: master\n VINS-Mono:\n type: git\n url: git@github.com:kelia/VINS-Mono.git\n version: imu_constant_integration\n" }, { "path": "fpv_aggressive_trajectories/.gitignore", "content": "cmake-build-debug/\n.idea/\n" }, { "path": "fpv_aggressive_trajectories/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(fpv_aggressive_trajectories)\n\n## Compile as C++11, supported in ROS Kinetic and newer\nadd_compile_options(-std=c++11)\nadd_compile_options(-O3)\n\nfind_package(catkin_simple REQUIRED)\ncatkin_simple(ALL_DEPS_REQUIRED)\n\ncs_add_library(visualize src/visualize.cpp)\n\ncs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp)\ntarget_include_directories(fpv_aggressive_trajectories PUBLIC\n ../../rpg_mpc/model/quadrotor_mpc_codegen/\n ../../rpg_mpc/externals/qpoases\n ../../rpg_mpc/externals/qpoases/INCLUDE\n ../../rpg_mpc/externals/qpoases/SRC)\ntarget_link_libraries(fpv_aggressive_trajectories visualize)\ncs_add_executable(odometry_republisher src/odometry_republisher.cpp)\n\ncs_install()\ncs_export()\n" }, { "path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/fpv_aggressive_trajectories.h", "content": "#pragma once\n\n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"fpv_aggressive_trajectories/visualize.h\"\n\nnamespace fpv_aggressive_trajectories {\n\nclass FPVAggressiveTrajectories {\n public:\n FPVAggressiveTrajectories(const ros::NodeHandle& nh,\n const ros::NodeHandle& pnh);\n\n FPVAggressiveTrajectories()\n : FPVAggressiveTrajectories(ros::NodeHandle(), ros::NodeHandle(\"~\")) {}\n\n virtual ~FPVAggressiveTrajectories();\n\n private:\n enum STATES {\n kOff = 0,\n kComputeTrajectory = 1,\n kExecuteTrajectory = 2,\n kNetworkControl = 3,\n kAutopilot = 4\n };\n ros::NodeHandle nh_;\n ros::NodeHandle pnh_;\n\n // Subscribers\n ros::Subscriber toggle_experiment_sub_;\n ros::Subscriber odometry_sub_;\n ros::Subscriber odometry_gt_sub_;\n\n // Publishers\n ros::Publisher control_command_pub_;\n ros::Publisher control_command_gt_pub_;\n ros::Publisher aggressive_end_pub_;\n ros::Publisher marker_pub_ref_;\n ros::Publisher vio_ref_pub_;\n ros::Publisher traj_comp_finish_pub_;\n\n quadrotor_common::QuadStateEstimate getPredictedStateEstimate(\n const ros::Time& time, const state_predictor::StatePredictor* predictor);\n\n void computeManeuver();\n\n quadrotor_common::ControlCommand computeControlCommand(\n const quadrotor_common::QuadStateEstimate& state_estimate,\n ros::Duration* trajectory_execution_left_duration,\n int* trajectories_left_in_queue);\n\n void publishControlCommand(\n const quadrotor_common::ControlCommand& control_command,\n const quadrotor_common::ControlCommand& control_command_gt);\n\n void startExecutionCallback(const std_msgs::BoolConstPtr& msg);\n\n void odometryCallback(const nav_msgs::OdometryConstPtr& msg);\n\n void odometryGTCallback(const nav_msgs::OdometryConstPtr& msg);\n\n bool loadParameters();\n\n std::list trajectory_queue_;\n // will be the same trajectories, but transformed to GT frame\n std::list trajectory_queue_gt_;\n\n // Parameters\n double desired_heading_;\n double exec_loop_rate_;\n double circle_velocity_;\n int n_loops_;\n nav_msgs::Odometry odometry_gt_;\n STATES state_machine_{kComputeTrajectory};\n ros::Time time_start_trajectory_execution_;\n quadrotor_common::QuadStateEstimate received_state_est_;\n quadrotor_common::QuadStateEstimate received_state_est_gt_;\n bool first_time_in_new_state_;\n double predictive_control_lookahead_ = 2.0;\n bool sent_maneuver_end_msg_ = false;\n int maneuver_counter_ = 0;\n double traj_sampling_freq_ = 50.0;\n\n // MPC controller variant\n rpg_mpc::MpcController base_controller_ =\n rpg_mpc::MpcController(ros::NodeHandle(), ros::NodeHandle(\"~\"),\n \"vio_mpc_path\");\n rpg_mpc::MpcParams base_controller_params_;\n state_predictor::StatePredictor state_predictor_;\n\n // Second controller instance that operates on ground truth data\n rpg_mpc::MpcParams base_controller_params_gt_;\n state_predictor::StatePredictor state_predictor_gt_;\n\n std::shared_ptr visualizer_;\n};\n\n} // namespace fpv_aggressive_trajectories\n" }, { "path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/odometry_republisher.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nnamespace odometry_republisher {\n\nclass OdometryRepublisher {\n public:\n OdometryRepublisher();\n virtual ~OdometryRepublisher();\n\n private:\n ros::NodeHandle nh_;\n\n ros::Subscriber state_estimate_sub_;\n ros::Publisher odometry_out_pub_;\n\n rpg::Pose T_B_S_;\n\n void odometryInCallback(const nav_msgs::OdometryConstPtr& msg);\n void transformOdometry(const nav_msgs::OdometryConstPtr& odom_in,\n nav_msgs::Odometry* odom_out);\n};\n\n} // namespace odometry_republisher\n" }, { "path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/visualize.h", "content": "#pragma once\n#include \n#include \n\n#include \"quadrotor_common/trajectory.h\"\n\nnamespace fpv_aggressive_trajectories {\nclass Visualizer {\n public:\n Visualizer(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);\n\n Visualizer() : Visualizer(ros::NodeHandle(), ros::NodeHandle(\"~\")) {}\n\n virtual ~Visualizer();\n\n void visualizeReferenceTrajectory(\n const quadrotor_common::Trajectory* trajectory);\n\n void visualizeTrajectories(\n const std::list& maneuver_list);\n\n void create_vehicle_markers(int num_rotors, float arm_len, float body_width,\n float body_height);\n\n void displayQuadrotor();\n\n private:\n ros::NodeHandle nh_;\n ros::NodeHandle pnh_;\n ros::Publisher bodyrates_viz_pub_;\n ros::Publisher marker_ref_pub_;\n ros::Publisher marker_pub_;\n ros::Publisher vehicle_marker_pub_;\n\n std::shared_ptr vehicle_marker_;\n};\n} // namespace fpv_aggressive_trajectories" }, { "path": "fpv_aggressive_trajectories/launch/simulation/controller.launch", "content": "\n\n\n \n \n \n \n \n\n \n \n \n \n \n \n \n\n\n" }, { "path": "fpv_aggressive_trajectories/launch/simulation/custom_rotors_interface.launch", "content": "\n\n\n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "fpv_aggressive_trajectories/launch/simulation/rpg_mpc.launch", "content": "\n\n\n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n\n\n \n \n \n \n\n \n \n \n \n \n\n \n \n \n\n\n \n \n \n \n \n\n\n" }, { "path": "fpv_aggressive_trajectories/launch/simulation/rpg_pid.launch", "content": "\n\n\n \n \n \n \n \n \n \n \n \n \n \n\n \n\n \n \n \n \n \n \n \n\n \n\n \n \n \n \n \n\n \n\n \n \n \n \n \n \n \n \n \n\n\n" }, { "path": "fpv_aggressive_trajectories/launch/simulation/simulation.launch", "content": "\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "fpv_aggressive_trajectories/package.xml", "content": "\n\n fpv_aggressive_trajectories\n 0.0.0\n The fpv_aggressive_trajectories package\n\n Elia Kaufmann\n TODO\n\n Elia Kaufmann\n\n catkin\n catkin_simple\n\n acrobatic_trajectory_helper\n autopilot\n eigen_catkin\n minimum_jerk_trajectories\n quadrotor_common\n quadrotor_msgs\n roscpp\n pose_utils\n rpg_mpc\n std_msgs\n nav_msgs\n\n" }, { "path": "fpv_aggressive_trajectories/parameters/mpc_params.yaml", "content": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Philipp Foehn, \n# Robotics and Perception Group, University of Zurich\n# \n# Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.\n# https://github.com/uzh-rpg/rpg_quadrotor_control\n# \n# This program is free software: you can redistribute it and/or modify\n# it under the terms of the GNU General Public License as published by\n# the Free Software Foundation, either version 3 of the License, or\n# (at your option) any later version.\n# \n# This program is distributed in the hope that it will be useful,\n# but WITHOUT ANY WARRANTY; without even the implied warranty of\n# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n# GNU General Public License for more details.\n# \n# You should have received a copy of the GNU General Public License\n# along with this program. If not, see .\n\n\n# Cost on states\nQ_pos_xy: 200 # Cost for horizontal positon error\nQ_pos_z: 500 # Cost for vertical position error\nQ_attitude: 50 # Cost for attitude error\nQ_velocity: 10 # Cost for velocity error\nQ_perception: 0 # Cost for reprojection error to camera optical axis (PAMPC)\n\n# Cost on Inputs\nR_thrust: 0.1 # Cost on thrust input\nR_pitchroll: 0.1 # Cost on pitch and roll rate\nR_yaw: 0.1 # Cost on yaw ratte\n\n# Exponential scaling: W_i = W * exp(-i/N * cost_scaling).\n# cost_scaling = 0 means no scaling W_i = W. \nstate_cost_exponential: 0.0 # Scaling for state costs\ninput_cost_exponential: 0.0 # scaling for input costs\n\n# Limits for inputs\nmax_bodyrate_xy: 20.0 # ~ pi [rad/s]\nmax_bodyrate_z: 5.0 # ~ pi*2/3 [rad/s]\nmin_thrust: 1.0 # ~ 20% gravity [N]\nmax_thrust: 40.0 # ~ 200% gravity [N]\n\n# Extrinsics for Perception Aware MPC\np_B_C: [ 0.0, 0.0, 0.0 ] # camera in body center [m]\nq_B_C: [ 0.6427876, 0, 0.7660444, 0 ] # 45° pitched down\n\n# Print information such as timing to terminal\nprint_info: false\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/asphalt.material", "content": "material iros/asphalt\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\tspecular 0.2 0.2 0.2 1.0 12.5\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture asphalt.png\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 0.1 0.1\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/carpet.material", "content": "material iros/carpet\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.2 0.2 0.2 1.0\n\t\t\tdiffuse 0.8 0.8 0.8 1.0\n\t\t\temissive 0.5 0.5 0.5 1\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture carpet.jpg\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 1.0 1.0\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_1.material", "content": "material iros/wall_1\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0.5 0.5 1\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture wall_1.jpeg\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 1.0 1.0\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_2.material", "content": "material iros/wall_2\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0.5 0.5 1\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture wall_2.jpeg\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 1.0 1.0\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_3.material", "content": "material iros/wall_3\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0.5 0.5 1\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture wall_3.jpeg\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 1.0 1.0\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_4.material", "content": "material iros/wall_4\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0.5 0.5 1\n\n\t\t\ttexture_unit\n\t\t\t{\n\t\t\t\ttexture wall_4.jpeg\n\t\t\t\tfiltering anistropic\n\t\t\t\tmax_anisotropy 16\n\t\t\t\tscale 1.0 1.0\n\t\t\t}\n\t\t}\n\t}\n}\n" }, { "path": "fpv_aggressive_trajectories/resources/race_track/real_world/gate/meshes/gate.dae", "content": "\r\n\r\n\r\n \r\n 2018-07-16T17:43:41\r\n 2018-07-16T17:43:41\r\n \r\n Z_UP\r\n \r\n\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n\r\n \r\n \r\n \r\n \r\n -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\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n 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\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n

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

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