├── gui └── rqt_quad_gui │ ├── src │ └── rqt_quad_gui │ │ ├── __init__.py │ │ ├── .gitignore │ │ ├── basic_flight.py │ │ ├── basic_flight_widget.py │ │ ├── quad_name_widget.py │ │ ├── gui_base.py │ │ └── quad_widget_common.py │ ├── scripts │ └── rqt_quad_gui │ ├── setup.py │ ├── package.xml │ ├── CMakeLists.txt │ ├── plugin.xml │ └── resource │ └── quad_name_widget.ui ├── documents └── theory_and_math │ ├── img │ ├── quadrotor.pdf │ ├── quad_picture.pdf │ ├── accelerometer.pdf │ ├── thrust_transient.pdf │ ├── kappa_load_cell_fpv.pdf │ ├── vbat_dep_function_id.pdf │ ├── vbat_dep_validation.pdf │ ├── thrust_mapping_fit_fpv.pdf │ ├── voltage_dep_thrust_mapping.pdf │ ├── quadrotor.tex │ ├── quadrotor.tikz │ └── accelerometer.pdf_tex │ ├── theory_and_math.pdf │ ├── .gitignore │ └── library.bib ├── quad_launch_files ├── CMakeLists.txt ├── package.xml └── launch │ ├── base_computer_example.launch │ └── real_quad_example.launch ├── utils ├── manual_flight_assistant │ ├── parameters │ │ └── default.yaml │ ├── CMakeLists.txt │ ├── launch │ │ └── manual_flight_assistant.launch │ ├── include │ │ └── manual_flight_assistant │ │ │ ├── rc_channels_switches.h │ │ │ ├── joypad_axes_buttons.h │ │ │ └── manual_flight_assistant.h │ ├── package.xml │ └── src │ │ └── manual_flight_assistant.cpp └── vbat_thrust_calibration │ ├── CMakeLists.txt │ ├── launch │ └── vbat_thrust_calibration.launch │ ├── package.xml │ └── scripts │ └── vbat_thrust_calibration.py ├── simulation └── rpg_rotors_interface │ ├── parameters │ ├── manual_flight_assistant.yaml │ ├── rpg_rotors_interface.yaml │ ├── position_controller.yaml │ └── autopilot.yaml │ ├── CMakeLists.txt │ ├── vehicles │ ├── components │ │ ├── color_camera.yaml │ │ └── color_camera.urdf.xacro │ ├── hummingbird_downlooking_camera.gazebo │ ├── hummingbird_frontlooking_camera.gazebo │ ├── hummingbird_downlooking_color_camera.gazebo │ └── hummingbird_frontlooking_color_camera.gazebo │ ├── launch │ ├── basics │ │ ├── rviz.launch │ │ └── base_quad_simulation.launch │ └── quadrotor_empty_world.launch │ ├── package.xml │ └── include │ └── rpg_rotors_interface │ └── rpg_rotors_interface.h ├── bridges └── sbus_bridge │ ├── src │ ├── sbus_bridge_node.cpp │ ├── thrust_mapping.cpp │ └── sbus_msg.cpp │ ├── msg │ └── SbusRosMessage.msg │ ├── CMakeLists.txt │ ├── include │ └── sbus_bridge │ │ ├── channel_mapping.h │ │ ├── thrust_mapping.h │ │ ├── sbus_serial_port.h │ │ ├── sbus_msg.h │ │ └── sbus_bridge.h │ ├── package.xml │ ├── launch │ └── sbus_bridge.launch │ └── parameters │ ├── race_quad.yaml │ └── default.yaml ├── control ├── autopilot │ ├── include │ │ └── autopilot │ │ │ ├── autopilot_states.h │ │ │ ├── autopilot_helper.h │ │ │ └── autopilot.h │ ├── src │ │ ├── autopilot_node.cpp │ │ └── autopilot_helper.cpp │ ├── CMakeLists.txt │ ├── package.xml │ └── parameters │ │ └── default.yaml └── position_controller │ ├── CMakeLists.txt │ ├── parameters │ ├── default.yaml │ └── fpv.yaml │ ├── package.xml │ └── include │ └── position_controller │ ├── position_controller.h │ └── position_controller_params.h ├── trajectory_planning ├── trajectory_generation_helper │ ├── CMakeLists.txt │ ├── include │ │ └── trajectory_generation_helper │ │ │ ├── heading_trajectory_helper.h │ │ │ ├── circle_trajectory_helper.h │ │ │ └── polynomial_trajectory_helper.h │ ├── package.xml │ └── src │ │ ├── heading_trajectory_helper.cpp │ │ ├── circle_trajectory_helper.cpp │ │ └── polynomial_trajectory_helper.cpp └── polynomial_trajectories │ ├── src │ ├── polynomial_trajectory.cpp │ └── constrained_polynomial_trajectories.cpp │ ├── CMakeLists.txt │ ├── package.xml │ └── include │ └── polynomial_trajectories │ ├── polynomial_trajectory_settings.h │ ├── polynomial_trajectory.h │ ├── polynomial_trajectories_common.h │ ├── constrained_polynomial_trajectories.h │ └── minimum_snap_trajectories.h ├── .gitignore ├── test └── rpg_quadrotor_integration_test │ ├── launch │ └── rpg_quadrotor_integration_test.launch │ ├── CMakeLists.txt │ ├── package.xml │ └── include │ └── rpg_quadrotor_integration_test │ └── rpg_quadrotor_integration_test.h ├── dependencies.yaml ├── LICENSE ├── .clang-format └── README.md /gui/rqt_quad_gui/src/rqt_quad_gui/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /documents/theory_and_math/img/quadrotor.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/quadrotor.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/quad_picture.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/quad_picture.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/theory_and_math.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/theory_and_math.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/accelerometer.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/accelerometer.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/thrust_transient.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/thrust_transient.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/kappa_load_cell_fpv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/kappa_load_cell_fpv.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/vbat_dep_function_id.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/vbat_dep_function_id.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/vbat_dep_validation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/vbat_dep_validation.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/thrust_mapping_fit_fpv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/thrust_mapping_fit_fpv.pdf -------------------------------------------------------------------------------- /documents/theory_and_math/img/voltage_dep_thrust_mapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uzh-rpg/rpg_quadrotor_control/HEAD/documents/theory_and_math/img/voltage_dep_thrust_mapping.pdf -------------------------------------------------------------------------------- /quad_launch_files/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(quad_launch_files) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | cs_install() 8 | cs_export() 9 | -------------------------------------------------------------------------------- /documents/theory_and_math/.gitignore: -------------------------------------------------------------------------------- 1 | .svn 2 | .aux 3 | .dvi 4 | .log 5 | .toc 6 | *~ 7 | *.synctex.gz 8 | *.bbl 9 | *.blg 10 | *.aux 11 | *.dvi 12 | *.log 13 | *.toc 14 | *.out 15 | *.bak 16 | .DS_Store 17 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/parameters/default.yaml: -------------------------------------------------------------------------------- 1 | joypad_timeout: 0.5 2 | sbus_timeout: 0.5 3 | joypad_axes_zero_tolerance: 0.05 4 | sbus_axes_zero_tolerance: 10 5 | vmax_xy: 1.5 6 | vmax_z: 0.7 7 | rmax_yaw: 1.5 8 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/parameters/manual_flight_assistant.yaml: -------------------------------------------------------------------------------- 1 | joypad_timeout: 0.5 2 | sbus_timeout: 0.5 3 | joypad_axes_zero_tolerance: 0.05 4 | sbus_axes_zero_tolerance: 10 5 | vmax_xy: 1.5 6 | vmax_z: 0.7 7 | rmax_yaw: 1.5 8 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/scripts/rqt_quad_gui: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | plugin = 'rqt_quad_gui' 8 | main = Main(filename=plugin) 9 | sys.exit(main.main(sys.argv, standalone=plugin)) 10 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/src/sbus_bridge_node.cpp: -------------------------------------------------------------------------------- 1 | #include "sbus_bridge/sbus_bridge.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "sbus_bridge"); 5 | sbus_bridge::SBusBridge sbus_bridge; 6 | 7 | ros::spin(); 8 | 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_quad_gui'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/msg/SbusRosMessage.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Regular 16 sbus channels with 11 bit value range each 4 | uint16[16] channels 5 | 6 | # Digital channels 7 | bool digital_channel_1 8 | bool digital_channel_2 9 | 10 | # Frame lost flag 11 | bool frame_lost 12 | 13 | # Failsafe flag 14 | bool failsafe 15 | -------------------------------------------------------------------------------- /utils/vbat_thrust_calibration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vbat_thrust_calibration) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_install() 12 | cs_export() 13 | -------------------------------------------------------------------------------- /control/autopilot/include/autopilot/autopilot_states.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace autopilot { 4 | 5 | enum class States { 6 | OFF, 7 | START, 8 | HOVER, 9 | LAND, 10 | EMERGENCY_LAND, 11 | BREAKING, 12 | GO_TO_POSE, 13 | VELOCITY_CONTROL, 14 | REFERENCE_CONTROL, 15 | TRAJECTORY_CONTROL, 16 | COMMAND_FEEDTHROUGH, 17 | RC_MANUAL 18 | }; 19 | 20 | } // namespace autopilot 21 | -------------------------------------------------------------------------------- /control/position_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(position_controller) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_library(${PROJECT_NAME} src/position_controller.cpp) 12 | 13 | cs_install() 14 | cs_export() 15 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpg_rotors_interface) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_executable(rpg_rotors_interface src/rpg_rotors_interface.cpp) 12 | 13 | cs_install() 14 | cs_export() 15 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(manual_flight_assistant) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_executable(manual_flight_assistant src/manual_flight_assistant.cpp) 12 | 13 | cs_install() 14 | cs_export() 15 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/launch/manual_flight_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/basic_flight.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from .gui_base import GuiBase 3 | from .basic_flight_widget import BasicFlightWidget 4 | 5 | 6 | class BasicFlight(GuiBase): 7 | """ 8 | Subclass of GuiBase to display Quad status 9 | """ 10 | def __init__(self, context): 11 | # Create QWidget 12 | widget = BasicFlightWidget() 13 | 14 | # Init GuiBase 15 | super(BasicFlight, self).__init__(context, widget) 16 | -------------------------------------------------------------------------------- /control/autopilot/src/autopilot_node.cpp: -------------------------------------------------------------------------------- 1 | #include "autopilot/autopilot.h" 2 | #include "position_controller/position_controller.h" 3 | #include "position_controller/position_controller_params.h" 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "autopilot"); 7 | autopilot::AutoPilot 9 | autopilot; 10 | 11 | ros::spin(); 12 | 13 | return 0; 14 | } 15 | -------------------------------------------------------------------------------- /control/position_controller/parameters/default.yaml: -------------------------------------------------------------------------------- 1 | position_controller: 2 | use_rate_mode: true 3 | 4 | kpxy: 10.0 5 | kdxy: 4.0 6 | 7 | kpz: 15.0 8 | kdz: 6.0 9 | krp: 12.0 10 | kyaw: 5.0 11 | 12 | pxy_error_max: 0.6 13 | vxy_error_max: 1.0 14 | pz_error_max: 0.3 15 | vz_error_max: 0.75 16 | yaw_error_max: 0.7 17 | 18 | perform_aerodynamics_compensation: false 19 | 20 | k_drag_x: 0.0 21 | k_drag_y: 0.0 22 | k_drag_z: 0.0 23 | k_thrust_horz: 0.0 24 | -------------------------------------------------------------------------------- /control/position_controller/parameters/fpv.yaml: -------------------------------------------------------------------------------- 1 | position_controller: 2 | use_rate_mode: true 3 | 4 | kpxy: 10.0 5 | kdxy: 4.0 6 | 7 | kpz: 20.0 8 | kdz: 6.0 9 | krp: 12.0 10 | kyaw: 4.0 11 | 12 | pxy_error_max: 0.6 13 | vxy_error_max: 1.0 14 | pz_error_max: 0.3 15 | vz_error_max: 1.0 16 | yaw_error_max: 0.7 17 | 18 | perform_aerodynamics_compensation: true 19 | 20 | k_drag_x: 0.544 21 | k_drag_y: 0.386 22 | k_drag_z: 0.0 23 | k_thrust_horz: 0.009 24 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/parameters/rpg_rotors_interface.yaml: -------------------------------------------------------------------------------- 1 | # Vehicle parameters 2 | inertia_x: 0.007 3 | inertia_y: 0.007 4 | inertia_z: 0.012 5 | arm_length: 0.17 6 | mass: 0.73 7 | # Motors parameters 8 | rotor_drag_coeff: 0.016 9 | rotor_thrust_coeff: 8.54858e-06 10 | max_rotor_speed: 838.0 11 | # Controller 12 | low_level_control_frequency: 200.0 13 | body_rates_p_xy: 0.1 14 | body_rates_d_xy: 0.5 15 | body_rates_p_z: 0.03 16 | body_rates_d_z: 0.1 17 | roll_pitch_cont_gain: 6.0 18 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/basic_flight_widget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from .quad_widget_common import QuadWidgetCommon 3 | 4 | from .autopilot_widget import AutopilotWidget 5 | 6 | 7 | class BasicFlightWidget(QuadWidgetCommon): 8 | def __init__(self): 9 | super(BasicFlightWidget, self).__init__() 10 | 11 | self._column_1.addWidget(self._name_widget) 12 | self._column_1.addWidget(AutopilotWidget(self)) 13 | 14 | self.setup_gui(two_columns=False) 15 | -------------------------------------------------------------------------------- /control/autopilot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autopilot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_executable(autopilot 12 | src/autopilot_node.cpp 13 | ) 14 | 15 | cs_add_library(autopilot_helper 16 | src/autopilot_helper.cpp 17 | ) 18 | 19 | cs_install() 20 | cs_export() 21 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sbus_bridge) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_executable(sbus_bridge src/sbus_bridge_node.cpp src/sbus_bridge.cpp 12 | src/sbus_serial_port.cpp src/sbus_msg.cpp src/thrust_mapping.cpp) 13 | 14 | cs_install() 15 | cs_export() 16 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/parameters/position_controller.yaml: -------------------------------------------------------------------------------- 1 | position_controller: 2 | use_rate_mode: true 3 | 4 | kpxy: 10.0 5 | kdxy: 4.0 6 | 7 | kpz: 15.0 8 | kdz: 6.0 9 | krp: 12.0 10 | kyaw: 5.0 11 | 12 | pxy_error_max: 0.6 13 | vxy_error_max: 1.0 14 | pz_error_max: 0.3 15 | vz_error_max: 0.75 16 | yaw_error_max: 0.7 17 | 18 | perform_aerodynamics_compensation: false 19 | 20 | k_drag_x: 0.0 21 | k_drag_y: 0.0 22 | k_drag_z: 0.0 23 | k_thrust_horz: 0.0 24 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/include/sbus_bridge/channel_mapping.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace sbus_bridge { 4 | 5 | namespace channel_mapping { 6 | 7 | static constexpr uint8_t kThrottle = 0; 8 | static constexpr uint8_t kRoll = 1; 9 | static constexpr uint8_t kPitch = 2; 10 | static constexpr uint8_t kYaw = 3; 11 | static constexpr uint8_t kArming = 4; 12 | static constexpr uint8_t kControlMode = 5; 13 | static constexpr uint8_t kGamepadMode = 6; 14 | 15 | } // namespace channel_mapping 16 | 17 | } // namespace sbus_bridge 18 | -------------------------------------------------------------------------------- /quad_launch_files/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | quad_launch_files 4 | 0.0.0 5 | The quad_launch_files package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /quad_launch_files/launch/base_computer_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(trajectory_generation_helper) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_library(${PROJECT_NAME} src/polynomial_trajectory_helper.cpp 12 | src/heading_trajectory_helper.cpp src/circle_trajectory_helper.cpp) 13 | 14 | cs_install() 15 | cs_export() 16 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/src/polynomial_trajectory.cpp: -------------------------------------------------------------------------------- 1 | #include "polynomial_trajectories/polynomial_trajectory.h" 2 | 3 | namespace polynomial_trajectories { 4 | 5 | PolynomialTrajectory::PolynomialTrajectory() 6 | : trajectory_type(TrajectoryType::UNDEFINED), 7 | coeff(), 8 | T(ros::Duration(0.0)), 9 | start_state(), 10 | end_state(), 11 | number_of_segments(0), 12 | segment_times(), 13 | optimization_cost(0.0) {} 14 | 15 | PolynomialTrajectory::~PolynomialTrajectory() {} 16 | 17 | } // namespace polynomial_trajectories 18 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(polynomial_trajectories) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | cs_add_library(${PROJECT_NAME} src/polynomial_trajectory.cpp 12 | src/polynomial_trajectories_common.cpp 13 | src/minimum_snap_trajectories.cpp 14 | src/constrained_polynomial_trajectories.cpp) 15 | 16 | cs_install() 17 | cs_export() 18 | -------------------------------------------------------------------------------- /utils/vbat_thrust_calibration/launch/vbat_thrust_calibration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # python 2 | *.pyc 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | 9 | # Compiled Dynamic libraries 10 | *.so 11 | *.dylib 12 | 13 | # Compiled Static libraries 14 | *.lai 15 | *.la 16 | *.a 17 | 18 | # Eclipse files 19 | .project 20 | .cproject 21 | .pydevproject 22 | .settings 23 | cmake_install.cmake 24 | 25 | # Temporary files 26 | *~ 27 | 28 | # Build Folders 29 | build 30 | bin 31 | lib 32 | srv_gen 33 | msg_gen 34 | *.cfgc 35 | 36 | # Log files 37 | log 38 | *.log 39 | 40 | ROS_NOBUILD 41 | 42 | # Matlab msgs gen 43 | matlab_gen 44 | 45 | # CLion 46 | .idea/ 47 | cmake-build-debug/ 48 | -------------------------------------------------------------------------------- /test/rpg_quadrotor_integration_test/launch/rpg_quadrotor_integration_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /test/rpg_quadrotor_integration_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpg_quadrotor_integration_test) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | find_package(rostest REQUIRED) 10 | catkin_simple(ALL_DEPS_REQUIRED) 11 | 12 | add_rostest_gtest( 13 | rpg_quadrotor_integration_test 14 | launch/rpg_quadrotor_integration_test.launch 15 | src/rpg_quadrotor_integration_test.cpp) 16 | target_link_libraries(rpg_quadrotor_integration_test ${catkin_LIBRARIES}) 17 | 18 | cs_install() 19 | cs_export() 20 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/include/trajectory_generation_helper/heading_trajectory_helper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace trajectory_generation_helper { 6 | 7 | namespace heading { 8 | 9 | void addConstantHeading(const double heading, 10 | quadrotor_common::Trajectory* trajectory); 11 | void addConstantHeadingRate(const double initial_heading, 12 | const double final_heading, 13 | quadrotor_common::Trajectory* trajectory); 14 | 15 | } // namespace heading 16 | 17 | } // namespace trajectory_generation_helper 18 | -------------------------------------------------------------------------------- /control/position_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | position_controller 4 | 0.0.0 5 | The position_controller package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | eigen_catkin 16 | quadrotor_common 17 | roscpp 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /utils/vbat_thrust_calibration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vbat_thrust_calibration 4 | 0.0.0 5 | The vbat_thrust_calibration package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | nav_msgs 16 | quadrotor_msgs 17 | rospy 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/include/manual_flight_assistant/rc_channels_switches.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace manual_flight_assistant { 4 | 5 | namespace rc { 6 | 7 | namespace channels { 8 | static constexpr uint32_t kX = 2; 9 | static constexpr uint32_t kY = 1; 10 | static constexpr uint32_t kZ = 0; 11 | static constexpr uint32_t kYaw = 3; 12 | } // namespace channels 13 | 14 | namespace switches { 15 | static constexpr uint32_t kSwitch1 = 0; 16 | static constexpr uint32_t kSwitch2 = 1; 17 | static constexpr uint32_t kSwitch3 = 2; 18 | static constexpr uint32_t kSwitch4 = 3; 19 | } // namespace switches 20 | 21 | } // namespace rc 22 | 23 | } // namespace manual_flight_assistant 24 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/components/color_camera.yaml: -------------------------------------------------------------------------------- 1 | cameras: 2 | - camera: 3 | distortion: 4 | parameters: 5 | cols: 1 6 | rows: 0 7 | data: [] 8 | type: none 9 | image_height: 480 10 | image_width: 640 11 | intrinsics: 12 | cols: 1 13 | rows: 4 14 | data: [268.5116780299728, 268.5116780299728, 320.5, 240.5] 15 | label: cam0 16 | line-delay-nanoseconds: 0 17 | type: pinhole 18 | T_B_C: 19 | cols: 4 20 | rows: 4 21 | data: [ 1., 0., 0., 0.0, 22 | 0., 1., 0., 0., 23 | 0., 0., 1., 0.0, 24 | 0., 0., 0., 1.] 25 | serial_no: 0 26 | calib_date: 0 27 | description: 'ROTORS' 28 | label: ROTORS 29 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sbus_bridge 4 | 0.0.0 5 | The sbus_bridge package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | eigen_catkin 16 | message_generation 17 | quadrotor_common 18 | quadrotor_msgs 19 | roscpp 20 | std_msgs 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | trajectory_generation_helper 4 | 0.0.0 5 | The trajectory_generation_helper package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | eigen_catkin 16 | polynomial_trajectories 17 | quadrotor_common 18 | roscpp 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | eigen_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/eigen_catkin.git 9 | version: master 10 | mav_comm: 11 | type: git 12 | url: git@github.com:ethz-asl/mav_comm.git 13 | version: master 14 | rotors_simulator: 15 | type: git 16 | url: git@github.com:ethz-asl/rotors_simulator.git 17 | version: master 18 | rpg_quadrotor_common: 19 | type: git 20 | url: git@github.com:uzh-rpg/rpg_quadrotor_common.git 21 | version: master 22 | rpg_single_board_io: 23 | type: git 24 | url: git@github.com:uzh-rpg/rpg_single_board_io.git 25 | version: master 26 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_quad_gui 4 | 0.0.0 5 | The rqt_quad_gui package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | Flavio Fontana 12 | 13 | catkin 14 | 15 | geometry_msgs 16 | quadrotor_msgs 17 | rospy 18 | rqt_gui 19 | rqt_gui_py 20 | std_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | polynomial_trajectories 4 | 0.0.0 5 | The polynomial_trajectories package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | Flavio Fontana 12 | 13 | catkin 14 | catkin_simple 15 | 16 | eigen_catkin 17 | quadrotor_common 18 | quadrotor_msgs 19 | roscpp 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/include/manual_flight_assistant/joypad_axes_buttons.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace manual_flight_assistant { 4 | 5 | namespace joypad { 6 | 7 | namespace axes { 8 | static constexpr uint32_t kX = 4; 9 | static constexpr uint32_t kY = 3; 10 | static constexpr uint32_t kZ = 1; 11 | static constexpr uint32_t kYaw = 0; 12 | } // namespace axes 13 | 14 | namespace buttons { 15 | static constexpr uint32_t kGreen = 0; 16 | static constexpr uint32_t kRed = 1; 17 | static constexpr uint32_t kBlue = 2; 18 | static constexpr uint32_t kYellow = 3; 19 | 20 | static constexpr uint32_t kLb = 4; 21 | static constexpr uint32_t kRb = 5; 22 | } // namespace buttons 23 | 24 | } // namespace joypad 25 | 26 | } // namespace manual_flight_assistant 27 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | manual_flight_assistant 4 | 0.0.0 5 | The manual_flight_assistant package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | geometry_msgs 16 | quadrotor_common 17 | roscpp 18 | sbus_bridge 19 | sensor_msgs 20 | std_msgs 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /documents/theory_and_math/img/quadrotor.tex: -------------------------------------------------------------------------------- 1 | \documentclass[crop]{standalone} 2 | 3 | \usepackage{float} 4 | \usepackage{listings} 5 | \usepackage{color} 6 | \usepackage{textcomp} 7 | \usepackage{graphicx} 8 | \usepackage{tikz} 9 | \usepackage{tikz-3dplot} 10 | \usepackage{relsize} 11 | 12 | \newcommand{\bVec}[1]{\mathbf{#1}} 13 | 14 | \newcommand{\vect}[3]{{_{\mathsmaller{\mathrm{#2}}}\mathbf{#1}_{\mathsmaller{\mathrm{#3}}}}} % vector: _{#2}_{#1}_{#3} 15 | 16 | \newcommand{\wfr}[0]{\ensuremath{W}} % world frame 17 | \newcommand{\bfr}[0]{\ensuremath{B}} % body frame 18 | \newcommand{\cfr}[0]{\ensuremath{C}} % C-frame 19 | 20 | \newcommand{\heading}[0]{\psi} 21 | 22 | \begin{document} 23 | 24 | \resizebox{7.8cm}{!}{ 25 | \input{quadrotor.tikz} 26 | } 27 | 28 | \end{document} 29 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/launch/sbus_bridge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rqt_quad_gui) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | add_compile_options(-O3) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | quadrotor_msgs 11 | rospy 12 | rqt_gui 13 | rqt_gui_py 14 | std_msgs 15 | ) 16 | 17 | catkin_python_setup() 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS geometry_msgs quadrotor_msgs rospy rqt_gui rqt_gui_py std_msgs 21 | ) 22 | 23 | install(FILES plugin.xml 24 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 25 | ) 26 | 27 | install(DIRECTORY resource 28 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 29 | ) 30 | 31 | install(PROGRAMS scripts/rqt_quad_gui 32 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 33 | ) 34 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/include/trajectory_generation_helper/circle_trajectory_helper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace trajectory_generation_helper { 7 | 8 | namespace circles { 9 | 10 | quadrotor_common::Trajectory computeHorizontalCircleTrajectory( 11 | const Eigen::Vector3d center, const double radius, const double speed, 12 | const double phi_start, const double phi_end, 13 | const double sampling_frequency); 14 | 15 | quadrotor_common::Trajectory computeVerticalCircleTrajectory( 16 | const Eigen::Vector3d center, const double orientation, const double radius, 17 | const double speed, const double phi_start, const double phi_end, 18 | const double sampling_frequency); 19 | 20 | } // namespace circles 21 | 22 | } // namespace trajectory_generation_helper 23 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/hummingbird_downlooking_camera.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /control/autopilot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autopilot 4 | 0.0.0 5 | The autopilot package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | 12 | catkin 13 | catkin_simple 14 | 15 | eigen_catkin 16 | geometry_msgs 17 | nav_msgs 18 | position_controller 19 | quadrotor_common 20 | quadrotor_msgs 21 | roscpp 22 | state_predictor 23 | std_msgs 24 | trajectory_generation_helper 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/hummingbird_frontlooking_camera.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10 | 11 | 12 | Plugin to fly a quadrotor with the rpg_quadrotor_control framework 13 | 14 | 15 | 16 | 17 | folder 18 | 19 | 20 | 21 | applications-other 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /control/autopilot/parameters/default.yaml: -------------------------------------------------------------------------------- 1 | state_estimate_timeout: 0.2 # [s] 2 | velocity_estimate_in_world_frame: true 3 | control_command_delay: 0.0 # [s] 4 | 5 | start_land_velocity: 0.5 # [m/s] 6 | start_land_acceleration: 1 # [m/s^2] 7 | start_idle_duration: 2.0 # [s] 8 | idle_thrust: 2.0 # [m/s] 9 | optitrack_start_height: 1.0 # [m] 10 | optitrack_start_land_timeout: 5 # [s] 11 | optitrack_land_drop_height: 0.3 # [m] 12 | propeller_ramp_down_timeout: 1.5 # [s] 13 | 14 | breaking_velocity_threshold: 0.2 # [m/s] 15 | breaking_timeout: 0.5 # [s] 16 | 17 | go_to_pose_max_velocity: 1.5 # [m/s] 18 | go_to_pose_max_normalized_thrust: 12.0 # [m/s^2] 19 | go_to_pose_max_roll_pitch_rate: 0.5 # [rad/s] 20 | 21 | velocity_command_input_timeout: 0.1 # [s] 22 | tau_velocity_command: 0.8 # [] 23 | 24 | reference_state_input_timeout: 0.1 # [s] 25 | 26 | emergency_land_duration: 4 # [s] 27 | emergency_land_thrust: 9.0 # [m/s^2] 28 | 29 | control_command_input_timeout: 0.1 # [s] 30 | enable_command_feedthrough: false 31 | predictive_control_lookahead: 2.0 # [s] 32 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/parameters/autopilot.yaml: -------------------------------------------------------------------------------- 1 | state_estimate_timeout: 0.1 # [s] 2 | velocity_estimate_in_world_frame: false 3 | control_command_delay: 0.05 # [s] 4 | 5 | start_land_velocity: 0.5 # [m/s] 6 | start_land_acceleration: 1 # [m/s^2] 7 | start_idle_duration: 2.0 # [s] 8 | idle_thrust: 2.0 # [m/s] 9 | optitrack_start_height: 1.0 # [m] 10 | optitrack_start_land_timeout: 5 # [s] 11 | optitrack_land_drop_height: 0.3 # [m] 12 | propeller_ramp_down_timeout: 1.5 # [s] 13 | 14 | breaking_velocity_threshold: 0.2 # [m/s] 15 | breaking_timeout: 0.5 # [s] 16 | 17 | go_to_pose_max_velocity: 1.5 # [m/s] 18 | go_to_pose_max_normalized_thrust: 12.0 # [m/s^2] 19 | go_to_pose_max_roll_pitch_rate: 0.5 # [rad/s] 20 | 21 | velocity_command_input_timeout: 0.1 # [s] 22 | tau_velocity_command: 0.8 # [] 23 | 24 | reference_state_input_timeout: 0.1 # [s] 25 | 26 | emergency_land_duration: 4 # [s] 27 | emergency_land_thrust: 9.0 # [m/s^2] 28 | 29 | control_command_input_timeout: 0.1 # [s] 30 | enable_command_feedthrough: true 31 | predictive_control_lookahead: 2.0 # [s] 32 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/launch/basics/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 19 | 20 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /test/rpg_quadrotor_integration_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpg_quadrotor_integration_test 4 | 0.0.0 5 | The rpg_quadrotor_integration_test package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | Titus Cieslewski 12 | 13 | catkin 14 | catkin_simple 15 | rostest 16 | 17 | autopilot 18 | eigen_catkin 19 | polynomial_trajectories 20 | quadrotor_common 21 | quadrotor_msgs 22 | roscpp 23 | rotors_description 24 | rotors_gazebo 25 | rpg_rotors_interface 26 | std_msgs 27 | trajectory_generation_helper 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/include/polynomial_trajectories/polynomial_trajectory_settings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "Eigen/Dense" 6 | 7 | namespace polynomial_trajectories { 8 | 9 | struct PolynomialTrajectorySettings { 10 | PolynomialTrajectorySettings() = default; 11 | 12 | PolynomialTrajectorySettings(const std::vector& way_points, 13 | const Eigen::VectorXd& minimization_weights, 14 | const int polynomial_order, 15 | const int continuity_order) 16 | : way_points(way_points), 17 | minimization_weights(minimization_weights), 18 | polynomial_order(polynomial_order), 19 | continuity_order(continuity_order) {} 20 | 21 | virtual ~PolynomialTrajectorySettings(){}; 22 | 23 | std::vector way_points; 24 | Eigen::VectorXd minimization_weights; 25 | int polynomial_order = 0; 26 | int continuity_order = 0; 27 | }; 28 | 29 | } // namespace polynomial_trajectories 30 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/hummingbird_downlooking_color_camera.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/hummingbird_frontlooking_color_camera.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/parameters/race_quad.yaml: -------------------------------------------------------------------------------- 1 | port_name: /dev/ttySAC0 2 | enable_receiving_sbus_messages: false 3 | control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' 4 | # set in the 'flight_controller'!) 5 | rc_timeout: 0.1 # [s] 6 | mass: 0.596 # [kg] 7 | disable_thrust_mapping: false 8 | # Note: When updating the thrust mapping also the voltage dependency mapping 9 | # has to be updated! 10 | # thrust = thrust_map_a * u^2 + thrust_map_b * u + thrust_map_c 11 | thrust_map_a: 7.13455275548e-06 12 | thrust_map_b: 0.0074241470842 13 | thrust_map_c: -6.03496490208 14 | # Maximum values for body rates and roll and pitch angles as they are set 15 | # on the Flight Controller. The max roll an pitch angles are only active 16 | # when flying in angle mode 17 | max_roll_rate: 1022.0 # [deg/s] 18 | max_pitch_rate: 1022.0 # [deg/s] 19 | max_yaw_rate: 524.0 # [deg/s] 20 | max_roll_angle: 50 # [deg] 21 | max_pitch_angle: 50 # [deg] 22 | alpha_vbat_filter: 0.01 23 | perform_thrust_voltage_compensation: true 24 | thrust_ratio_voltage_map_a: -0.17220303 # [1/V] 25 | thrust_ratio_voltage_map_b: 3.13990035 # [-] 26 | n_lipo_cells: 3 # [-] -------------------------------------------------------------------------------- /bridges/sbus_bridge/parameters/default.yaml: -------------------------------------------------------------------------------- 1 | port_name: /dev/ttyUSB0 2 | enable_receiving_sbus_messages: true 3 | control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' 4 | # set in the 'flight_controller'!) 5 | rc_timeout: 0.1 # [s] 6 | mass: 0.865 # [kg] 7 | disable_thrust_mapping: false 8 | # Note: When updating the thrust mapping also the voltage dependency mapping 9 | # has to be updated! 10 | # thrust = thrust_map_a * u^2 + thrust_map_b * u + thrust_map_c 11 | thrust_map_a: 6.91194111204e-06 # 12 | thrust_map_b: 0.00754094874204 # 13 | thrust_map_c: -6.01740637316 # 14 | # Maximum values for body rates and roll and pitch angles as they are set 15 | # on the Flight Controller. The max roll an pitch angles are only active 16 | # when flying in angle mode 17 | max_roll_rate: 804.0 # [deg/s] 18 | max_pitch_rate: 804.0 # [deg/s] 19 | max_yaw_rate: 400.0 # [deg/s] 20 | max_roll_angle: 50 # [deg] 21 | max_pitch_angle: 50 # [deg] 22 | alpha_vbat_filter: 0.01 23 | perform_thrust_voltage_compensation: true 24 | thrust_ratio_voltage_map_a: -0.17044342 # [1/V] 25 | thrust_ratio_voltage_map_b: 3.10495276 # [-] 26 | n_lipo_cells: 3 # [-] 27 | -------------------------------------------------------------------------------- /test/rpg_quadrotor_integration_test/include/rpg_quadrotor_integration_test/rpg_quadrotor_integration_test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rpg_quadrotor_integration_test { 8 | 9 | class QuadrotorIntegrationTest { 10 | public: 11 | QuadrotorIntegrationTest(); 12 | virtual ~QuadrotorIntegrationTest(); 13 | 14 | void run(); 15 | 16 | private: 17 | void measureTracking(const ros::TimerEvent& time); 18 | 19 | ros::NodeHandle nh_; 20 | 21 | ros::Publisher arm_pub_; 22 | 23 | ros::Subscriber autopilot_feedback_sub_; 24 | 25 | ros::Timer measure_tracking_timer_; 26 | 27 | autopilot_helper::AutoPilotHelper autopilot_helper_; 28 | bool executing_trajectory_; 29 | 30 | // Performance metrics variables 31 | double sum_position_error_squared_; 32 | double max_position_error_; 33 | double sum_thrust_direction_error_squared_; 34 | double max_thrust_direction_error_; 35 | 36 | // Constants 37 | static constexpr double kExecLoopRate_ = 50.0; 38 | }; 39 | 40 | } // namespace rpg_quadrotor_integration_test 41 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/include/polynomial_trajectories/polynomial_trajectory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace polynomial_trajectories { 10 | 11 | enum class TrajectoryType { 12 | UNDEFINED, 13 | FULLY_CONSTRAINED, 14 | MINIMUM_SNAP, 15 | MINIMUM_SNAP_RING, 16 | MINIMUM_SNAP_OPTIMIZED_SEGMENTS, 17 | MINIMUM_SNAP_RING_OPTIMIZED_SEGMENTS 18 | }; 19 | 20 | struct PolynomialTrajectory { 21 | PolynomialTrajectory(); 22 | virtual ~PolynomialTrajectory(); 23 | 24 | TrajectoryType trajectory_type; 25 | // Polynomial coefficients 26 | // Each element of this vector contains the coefficients for one polynomial 27 | // segment (rows: dimension, columns: order) 28 | std::vector coeff; 29 | ros::Duration T; 30 | quadrotor_common::TrajectoryPoint start_state; 31 | quadrotor_common::TrajectoryPoint end_state; 32 | int number_of_segments; 33 | Eigen::VectorXd segment_times; 34 | double optimization_cost; 35 | }; 36 | 37 | } // namespace polynomial_trajectories 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Matthias Faessler (Robotics and Perception Group, 4 | University of Zurich, Switzerland) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpg_rotors_interface 4 | 0.0.0 5 | The rpg_rotors_interface package 6 | 7 | Matthias Faessler 8 | MIT 9 | 10 | Matthias Faessler 11 | Davide Falanga 12 | 13 | catkin 14 | catkin_simple 15 | 16 | eigen_catkin 17 | mav_msgs 18 | nav_msgs 19 | quadrotor_common 20 | quadrotor_msgs 21 | roscpp 22 | std_srvs 23 | 24 | 25 | 30 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/include/sbus_bridge/thrust_mapping.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace thrust_mapping { 6 | 7 | class CollectiveThrustMapping { 8 | public: 9 | CollectiveThrustMapping(); 10 | CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, 11 | const double thrust_map_c, 12 | const bool perform_thrust_voltage_compensation, 13 | const double thrust_ratio_voltage_map_a, 14 | const double thrust_ratio_voltage_map_b, 15 | const int n_lipo_cells); 16 | 17 | virtual ~CollectiveThrustMapping(); 18 | 19 | uint16_t inverseThrustMapping(const double thrust, 20 | const double battery_voltage) const; 21 | 22 | bool loadParameters(); 23 | 24 | private: 25 | double thrust_map_a_; 26 | double thrust_map_b_; 27 | double thrust_map_c_; 28 | 29 | bool perform_thrust_voltage_compensation_; 30 | double thrust_ratio_voltage_map_a_; 31 | double thrust_ratio_voltage_map_b_; 32 | int n_lipo_cells_; 33 | 34 | // Constants 35 | static constexpr double kMinBatteryCompensationVoltagePerCell_ = 3.5; 36 | static constexpr double kMaxBatteryCompensationVoltagePerCell_ = 4.2; 37 | }; 38 | 39 | } // namespace thrust_mapping 40 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/include/sbus_bridge/sbus_serial_port.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sbus_bridge/sbus_msg.h" 7 | 8 | namespace sbus_bridge { 9 | 10 | class SBusSerialPort { 11 | public: 12 | SBusSerialPort(); 13 | SBusSerialPort(const std::string& port, const bool start_receiver_thread); 14 | virtual ~SBusSerialPort(); 15 | 16 | protected: 17 | bool setUpSBusSerialPort(const std::string& port, 18 | const bool start_receiver_thread); 19 | 20 | bool connectSerialPort(const std::string& port); 21 | void disconnectSerialPort(); 22 | 23 | bool startReceiverThread(); 24 | bool stopReceiverThread(); 25 | 26 | void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; 27 | virtual void handleReceivedSbusMessage( 28 | const sbus_bridge::SBusMsg& received_sbus_msg) = 0; 29 | 30 | private: 31 | static constexpr int kSbusFrameLength_ = 25; 32 | static constexpr uint8_t kSbusHeaderByte_ = 0x0F; 33 | static constexpr uint8_t kSbusFooterByte_ = 0x00; 34 | static constexpr int kPollTimeoutMilliSeconds_ = 500; 35 | 36 | bool configureSerialPortForSBus() const; 37 | void serialPortReceiveThread(); 38 | sbus_bridge::SBusMsg parseSbusMessage( 39 | uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; 40 | 41 | std::thread receiver_thread_; 42 | std::atomic_bool receiver_thread_should_exit_; 43 | 44 | int serial_port_fd_; 45 | }; 46 | 47 | } // namespace sbus_bridge 48 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/quad_name_widget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import rospy 4 | import rospkg 5 | 6 | from python_qt_binding import loadUi 7 | try: 8 | # Starting from Qt 5 QWidget is defined in QtWidgets and not QtGui anymore 9 | from python_qt_binding.QtWidgets import QWidget 10 | except: 11 | from python_qt_binding.QtGui import QWidget 12 | from python_qt_binding.QtCore import QTimer, Slot 13 | from python_qt_binding.QtCore import pyqtSlot 14 | 15 | 16 | class QuadNameWidget(QWidget): 17 | 18 | def __init__(self, parent): 19 | super(QuadNameWidget, self).__init__(parent) 20 | self._parent = parent 21 | self._connected = False 22 | 23 | # load UI 24 | ui_file = os.path.join( 25 | os.path.dirname(os.path.realpath(__file__)), 26 | '../../resource/quad_name_widget.ui') 27 | loadUi(ui_file, self) 28 | 29 | def getQuadName(self): 30 | return self.namespace_text.text() 31 | 32 | def setQuadName(self, quadname): 33 | self.namespace_text.setText(quadname) 34 | 35 | @Slot(bool) 36 | def on_button_connect_clicked(self): 37 | if(self._connected): 38 | self.button_connect.setText('Connect') 39 | self._parent.disconnect() 40 | self._connected = False 41 | else: 42 | self.button_connect.setText('Disconnect') 43 | self._parent.connect() 44 | self._connected = True 45 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/include/polynomial_trajectories/polynomial_trajectories_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "polynomial_trajectories/polynomial_trajectory.h" 8 | 9 | namespace polynomial_trajectories { 10 | 11 | quadrotor_common::TrajectoryPoint getPointFromTrajectory( 12 | const PolynomialTrajectory& trajectory, 13 | const ros::Duration& time_from_start); 14 | Eigen::VectorXd computeFactorials(const int length, const int order); 15 | Eigen::VectorXd dVec(const int number_of_coefficients, 16 | const int derivative_order); 17 | Eigen::VectorXd tVec(const int number_of_coefficients, 18 | const int derivative_order, const double t); 19 | void computeMaxima(const PolynomialTrajectory& trajectory, 20 | double* maximal_velocity, double* maximal_acceleration, 21 | double* maximal_jerk, double* maximal_snap); 22 | void computeQuadRelevantMaxima(const PolynomialTrajectory& trajectory, 23 | double* maximal_velocity, 24 | double* maximal_normalized_thrust, 25 | double* maximal_roll_pitch_rate); 26 | bool isStartAndEndStateFeasibleUnderConstraints( 27 | const quadrotor_common::TrajectoryPoint& start_state, 28 | const quadrotor_common::TrajectoryPoint& end_state, 29 | const double max_velocity, const double max_normalized_thrust, 30 | const double max_roll_pitch_rate); 31 | double computeRollPitchRateNormFromTrajectoryPoint( 32 | const quadrotor_common::TrajectoryPoint& desired_state); 33 | 34 | } // namespace polynomial_trajectories 35 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/src/heading_trajectory_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory_generation_helper/heading_trajectory_helper.h" 2 | 3 | #include 4 | 5 | namespace trajectory_generation_helper { 6 | 7 | namespace heading { 8 | 9 | void addConstantHeading(const double heading, 10 | quadrotor_common::Trajectory* trajectory) { 11 | std::list::iterator it; 12 | for (it = trajectory->points.begin(); it != trajectory->points.end(); it++) { 13 | it->heading = heading; 14 | it->heading_rate = 0.0; 15 | it->heading_acceleration = 0.0; 16 | } 17 | } 18 | 19 | void addConstantHeadingRate(const double initial_heading, 20 | const double final_heading, 21 | quadrotor_common::Trajectory* trajectory) { 22 | if (trajectory->points.size() < 2) { 23 | return; 24 | } 25 | const double delta_angle = 26 | quadrotor_common::wrapAngleDifference(initial_heading, final_heading); 27 | const double trajectory_duration = 28 | (trajectory->points.back().time_from_start - 29 | trajectory->points.front().time_from_start) 30 | .toSec(); 31 | 32 | const double heading_rate = delta_angle / trajectory_duration; 33 | 34 | std::list::iterator it; 35 | for (it = trajectory->points.begin(); it != trajectory->points.end(); it++) { 36 | const double duration_ratio = 37 | (it->time_from_start - trajectory->points.front().time_from_start) 38 | .toSec() / 39 | trajectory_duration; 40 | it->heading = initial_heading + duration_ratio * delta_angle; 41 | it->heading_rate = heading_rate; 42 | it->heading_acceleration = 0.0; 43 | } 44 | } 45 | 46 | } // namespace heading 47 | 48 | } // namespace trajectory_generation_helper 49 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/include/sbus_bridge/sbus_msg.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "sbus_bridge/SbusRosMessage.h" 6 | 7 | namespace sbus_bridge { 8 | 9 | enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; 10 | 11 | enum class ArmState { DISARMED, ARMED }; 12 | 13 | #pragma pack(push) 14 | #pragma pack(1) 15 | struct SBusMsg { 16 | // Constants 17 | static constexpr int kNChannels = 16; 18 | static constexpr uint16_t kMinCmd = 192; // corresponds to 1000 on FC 19 | static constexpr uint16_t kMeanCmd = 992; // corresponds to 1500 on FC 20 | static constexpr uint16_t kMaxCmd = 1792; // corresponds to 2000 on FC 21 | 22 | ros::Time timestamp; 23 | 24 | // Normal 11 bit channels 25 | uint16_t channels[kNChannels]; 26 | 27 | // Digital channels (ch17 and ch18) 28 | bool digital_channel_1; 29 | bool digital_channel_2; 30 | 31 | // Flags 32 | bool frame_lost; 33 | bool failsafe; 34 | 35 | SBusMsg(); 36 | SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg); 37 | virtual ~SBusMsg(); 38 | 39 | sbus_bridge::SbusRosMessage toRosMessage() const; 40 | 41 | void limitAllChannelsFeasible(); 42 | void limitSbusChannelFeasible(const int channel_idx); 43 | 44 | // Setting sbus command helpers 45 | void setThrottleCommand(const uint16_t throttle_cmd); 46 | void setRollCommand(const uint16_t roll_cmd); 47 | void setPitchCommand(const uint16_t pitch_cmd); 48 | void setYawCommand(const uint16_t yaw_cmd); 49 | void setControlMode(const ControlMode& control_mode); 50 | void setControlModeAttitude(); 51 | void setControlModeBodyRates(); 52 | void setArmState(const ArmState& arm_state); 53 | void setArmStateArmed(); 54 | void setArmStateDisarmed(); 55 | 56 | // Sbus message check helpers 57 | bool isArmed() const; 58 | ControlMode getControlMode() const; 59 | }; 60 | #pragma pack(pop) 61 | } // namespace sbus_bridge 62 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/include/polynomial_trajectories/constrained_polynomial_trajectories.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "polynomial_trajectories/polynomial_trajectory.h" 7 | 8 | namespace polynomial_trajectories { 9 | 10 | namespace constrained_polynomial_trajectories { 11 | 12 | // order_of_continuity: 1 = position, 2 = velocity, .... 13 | 14 | PolynomialTrajectory computeTimeOptimalTrajectory( 15 | const quadrotor_common::TrajectoryPoint& s0, 16 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 17 | const double max_velocity, const double max_normalized_thrust, 18 | const double max_roll_pitch_rate); 19 | 20 | PolynomialTrajectory computeFixedTimeTrajectory( 21 | const quadrotor_common::TrajectoryPoint& s0, 22 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 23 | const double execution_time); 24 | 25 | // these functions should not be used from the outside 26 | namespace implementation { 27 | Eigen::MatrixXd computeConstraintMatriceA(const int order_of_continuity, 28 | const double t); 29 | 30 | Eigen::VectorXd computeConstraintMatriceB( 31 | const int order_of_continuity, 32 | const quadrotor_common::TrajectoryPoint& state, const int axis); 33 | 34 | std::vector computeTrajectoryCoeff( 35 | const quadrotor_common::TrajectoryPoint& s0, 36 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 37 | const double T); 38 | 39 | Eigen::Vector3d computeMaximaGradient(const PolynomialTrajectory& trajectory, 40 | const Eigen::Vector3d& maxima, 41 | const double order_of_continuity); 42 | } // namespace implementation 43 | 44 | } // namespace constrained_polynomial_trajectories 45 | 46 | } // namespace polynomial_trajectories 47 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/gui_base.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import rospy 4 | import argparse 5 | from qt_gui.plugin import Plugin 6 | 7 | 8 | class GuiBase(Plugin): 9 | """ 10 | Subclass of Plugin to display Quad status 11 | """ 12 | def __init__(self, context, main_qt_widget): 13 | # Init Plugin 14 | super(GuiBase, self).__init__(context) # Init Plugin superclass 15 | self.setObjectName('RPG Quad Gui Plugin') 16 | 17 | # Load arguments 18 | args = self._parse_args(context.argv()) 19 | 20 | # Create QWidget 21 | self._widget = main_qt_widget 22 | 23 | # Set quad_name 24 | self._widget.setQuadName("/" + args.quad_name) 25 | 26 | # Show _widget.windowTitle on left-top of each plugin (when 27 | # it's set in _widget). This is useful when you open multiple 28 | # plugins at once. Also if you open multiple instances of your 29 | # plugin at once, these lines add number to make it easy to 30 | # tell from pane to pane. 31 | if context.serial_number() > 1: 32 | self._widget.setWindowTitle( 33 | 'RPG Quad GUI' + (' (%d)' % context.serial_number())) 34 | else: 35 | self._widget.setWindowTitle('RPG Quad GUI') 36 | # Add widget to the user interface 37 | context.add_widget(self._widget) 38 | 39 | def _parse_args(self, argv): 40 | parser = argparse.ArgumentParser() 41 | parser.add_argument( 42 | '--quad_name', help='quadrotor name', default='quad_name') 43 | args, unknown = parser.parse_known_args() 44 | return args 45 | 46 | def shutdown_plugin(self): 47 | self._widget.disconnect() 48 | 49 | def save_settings(self, plugin_settings, instance_settings): 50 | # Save instance settings here 51 | pass 52 | 53 | def restore_settings(self, plugin_settings, instance_settings): 54 | # Restore instance settings here after gui has been newly launched 55 | pass 56 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/launch/quadrotor_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/include/manual_flight_assistant/manual_flight_assistant.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace manual_flight_assistant { 9 | 10 | class ManualFlightAssistant { 11 | public: 12 | ManualFlightAssistant(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 13 | ManualFlightAssistant() 14 | : ManualFlightAssistant(ros::NodeHandle(), ros::NodeHandle("~")) {} 15 | virtual ~ManualFlightAssistant(); 16 | 17 | private: 18 | void mainLoop(const ros::TimerEvent& time); 19 | 20 | void rcSbusCallback(const sbus_bridge::SbusRosMessage::ConstPtr& msg); 21 | void joyCallback(const sensor_msgs::Joy::ConstPtr& msg); 22 | 23 | bool joypadAvailable(); 24 | bool rcSbusAvailable(); 25 | 26 | void publishVelocityCommand( 27 | const geometry_msgs::TwistStamped& velocity_command); 28 | 29 | bool loadParameters(); 30 | 31 | ros::NodeHandle nh_; 32 | ros::NodeHandle pnh_; 33 | 34 | ros::Publisher manual_desired_velocity_pub_; 35 | ros::Publisher start_pub_; 36 | ros::Publisher land_pub_; 37 | 38 | ros::Subscriber joypad_sub_; 39 | ros::Subscriber rc_sbus_sub_; 40 | 41 | ros::Timer main_loop_timer_; 42 | 43 | sensor_msgs::Joy joypad_command_; 44 | sensor_msgs::Joy previous_joypad_command_; 45 | ros::Time time_last_joypad_msg_; 46 | 47 | sbus_bridge::SbusRosMessage sbus_command_; 48 | sbus_bridge::SbusRosMessage previous_sbus_command_; 49 | ros::Time time_last_sbus_msg_; 50 | bool sbus_needs_to_go_through_zero_; 51 | bool velocity_command_is_zero_; 52 | 53 | // Parameters 54 | double joypad_timeout_; 55 | double sbus_timeout_; 56 | double joypad_axes_zero_tolerance_; 57 | int sbus_axes_zero_tolerance_; 58 | double vmax_xy_; 59 | double vmax_z_; 60 | double rmax_yaw_; 61 | 62 | // Constants 63 | static constexpr double kLoopFrequency_ = 50.0; 64 | static constexpr double kVelocityCommandZeroThreshold_ = 0.03; 65 | }; 66 | 67 | } // namespace manual_flight_assistant 68 | -------------------------------------------------------------------------------- /documents/theory_and_math/img/quadrotor.tikz: -------------------------------------------------------------------------------- 1 | \tdplotsetmaincoords{65}{10} 2 | \begin{tikzpicture}[tdplot_main_coords, scale=2] 3 | 4 | % frame 5 | \draw[very thick] (-1.4,0,1) -- (1.4,0,1); 6 | \draw[very thick] (0,-1.4,1) -- (0,1.4,1); 7 | 8 | % rotors 9 | \draw[draw=none, fill=gray!40, opacity=0.8] (0,1.4,1) circle (0.8); 10 | \draw[draw=none, fill=gray!40, opacity=0.8] (1.4,0,1) circle (0.8); 11 | \draw[draw=none, fill=gray!40, opacity=0.8] (0,-1.4,1) circle (0.8); 12 | \draw[draw=none, fill=gray!40, opacity=0.8] (-1.4,0,1) circle (0.8); 13 | 14 | % forces 15 | \draw[very thick,->] (0,1.4,1) -- (0,1.4,1.5); 16 | \draw[very thick,->] (1.4,0,1) -- (1.4,0,1.5); 17 | \draw[very thick,->] (0,-1.4,1) -- (0,-1.4,1.5); 18 | \draw[very thick,->] (-1.4,0,1) -- (-1.4,0,1.5); 19 | 20 | % propeller rotation 21 | \draw[thick,->] (0,2.0,1) arc (90:0:0.6); 22 | \draw[thick,<-] (2.0,0,1) arc (0:-90:0.6); 23 | \draw[thick,->] (0.6,-1.4,1) arc (0:-90:0.6); 24 | \draw[thick,->] (-2.0,0,1) arc (180:270:0.6); 25 | 26 | % quad frame 27 | \draw[thick,->,color=red,text=black] (0,0,1) -- (0.7071,0.7071,1) node[right] {$\vect{x}{}{\bfr}$}; 28 | \draw[thick,->,color=green,text=black] (0,0,1) -- (-0.7071,0.7071,1) node[above] {$\vect{y}{}{\bfr}$}; 29 | \draw[thick,->,color=blue,text=black] (0,0,1) -- (0,0,2) node[left] {$\vect{z}{}{\bfr}$}; 30 | \node[draw=none] at (0.2,0,0.85) {Body}; 31 | 32 | % world frame 33 | \draw[thick,->,color=red,text=black] (-1.5,-1,-0.5) -- (-0.7,-1,-0.5) node[right] {$\vect{x}{}{\wfr}$}; 34 | \draw[thick,->,color=green,text=black] (-1.5,-1,-0.5) -- (-1.5,-0.2,-0.5) node[right] {$\vect{y}{}{\wfr}$}; 35 | \draw[thick,->,color=blue,text=black] (-1.5,-1,-0.5) -- (-1.5,-1,0.3) node[above] {$\vect{z}{}{\wfr} = \vect{z}{}{\cfr}$}; 36 | \node[draw=none] at (-1.6,0,-1.1) {World}; 37 | 38 | % C-frame 39 | \draw[thick,dashed,->,color=red,text=black] (-1.5,-1,-0.5) -- (-0.9343,-0.4343,-0.5) node[right] {$\vect{x}{}{\cfr}$}; 40 | \draw[thick,dashed,->,color=green,text=black] (-1.5,-1,-0.5) -- (-2.0657,-0.4343,-0.5) node[above] {$\vect{y}{}{\cfr}$}; 41 | 42 | % Yaw 43 | \draw[thin,->] (-1.0,-1,-0.5) arc (0:45:0.5) node[midway, right] {$\heading$}; 44 | 45 | % gravity 46 | \draw[very thick,->,color=black,text=black] (2,-1.0,0.8) -- node[right] {$-g \vect{z}{}{\wfr}$} (2,-1.0,0); 47 | 48 | \end{tikzpicture} 49 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/include/rpg_rotors_interface/rpg_rotors_interface.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace rpg_rotors_interface { 12 | 13 | struct TorquesAndThrust { 14 | Eigen::Vector3d body_torques; 15 | double collective_thrust; 16 | }; 17 | 18 | class RPGRotorsInterface { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | RPGRotorsInterface(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 23 | 24 | RPGRotorsInterface() 25 | : RPGRotorsInterface(ros::NodeHandle(), ros::NodeHandle("~")) {} 26 | ~RPGRotorsInterface(); 27 | 28 | private: 29 | void lowLevelControlLoop(const ros::TimerEvent& time); 30 | 31 | void rotorsOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg); 32 | void rpgControlCommandCallback( 33 | const quadrotor_msgs::ControlCommand::ConstPtr& msg); 34 | void motorSpeedCallback(const mav_msgs::Actuators::ConstPtr& msg); 35 | 36 | quadrotor_common::ControlCommand attitudeControl( 37 | const quadrotor_common::ControlCommand& attitude_cmd, 38 | const Eigen::Quaterniond& attitude_estimate); 39 | 40 | TorquesAndThrust bodyRateControl( 41 | const quadrotor_common::ControlCommand& rate_cmd, 42 | const Eigen::Vector3d& body_rate_estimate); 43 | 44 | mav_msgs::Actuators mixer(const TorquesAndThrust& torques_and_thrust); 45 | 46 | void armInterfaceCallback(const std_msgs::Bool::ConstPtr& msg); 47 | 48 | void loadParameters(); 49 | 50 | nav_msgs::Odometry quad_state_; 51 | 52 | ros::NodeHandle nh_; 53 | ros::NodeHandle pnh_; 54 | 55 | ros::Timer low_level_control_loop_timer_; 56 | 57 | ros::Publisher rotors_desired_motor_speed_pub_; 58 | 59 | ros::Subscriber rotors_odometry_sub_; 60 | ros::Subscriber rpg_control_command_sub_; 61 | ros::Subscriber motor_speed_sub_; 62 | ros::Subscriber arm_interface_sub_; 63 | 64 | bool interface_armed_; 65 | TorquesAndThrust torques_and_thrust_estimate_; 66 | quadrotor_msgs::ControlCommand control_command_; 67 | mav_msgs::Actuators last_motor_speed_; 68 | 69 | // Parameters 70 | Eigen::Matrix3d inertia_; 71 | Eigen::MatrixXd K_lqr_; 72 | double low_level_control_frequency_; 73 | double roll_pitch_cont_gain_; 74 | double arm_length_; 75 | double rotor_drag_coeff_; 76 | double rotor_thrust_coeff_; 77 | double mass_; 78 | double max_rotor_speed_; 79 | }; 80 | 81 | } // namespace rpg_rotors_interface 82 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/resource/quad_name_widget.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | QuadWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 80 11 | 12 | 13 | 14 | 15 | 0 16 | 0 17 | 18 | 19 | 20 | 21 | 400 22 | 80 23 | 24 | 25 | 26 | Copilot 27 | 28 | 29 | 30 | 31 | 10 32 | 10 33 | 291 34 | 31 35 | 36 | 37 | 38 | 39 | 18 40 | 75 41 | true 42 | 43 | 44 | 45 | RPG Quadrotor Gui 46 | 47 | 48 | 49 | 50 | 51 | 310 52 | 50 53 | 80 54 | 27 55 | 56 | 57 | 58 | Connect 59 | 60 | 61 | 62 | 63 | 64 | 10 65 | 50 66 | 141 67 | 27 68 | 69 | 70 | 71 | 72 | 0 73 | 0 74 | 75 | 76 | 77 | 78 | 11 79 | 80 | 81 | 82 | Quad Namespace: 83 | 84 | 85 | 86 | 87 | 88 | 150 89 | 50 90 | 159 91 | 27 92 | 93 | 94 | 95 | Topic to send Twist message to 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /control/position_controller/include/position_controller/position_controller.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "position_controller/position_controller_params.h" 11 | 12 | namespace position_controller { 13 | 14 | class PositionController { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | PositionController(); 19 | ~PositionController(); 20 | 21 | quadrotor_common::ControlCommand off(); 22 | quadrotor_common::ControlCommand run( 23 | const quadrotor_common::QuadStateEstimate& state_estimate, 24 | const quadrotor_common::Trajectory& reference_trajectory, 25 | const PositionControllerParams& config); 26 | 27 | private: 28 | quadrotor_common::ControlCommand computeNominalReferenceInputs( 29 | const quadrotor_common::TrajectoryPoint& reference_state, 30 | const Eigen::Quaterniond& attitude_estimate) const; 31 | 32 | void computeAeroCompensatedReferenceInputs( 33 | const quadrotor_common::TrajectoryPoint& reference_state, 34 | const quadrotor_common::QuadStateEstimate& state_estimate, 35 | const PositionControllerParams& config, 36 | quadrotor_common::ControlCommand* reference_inputs, 37 | Eigen::Vector3d* drag_accelerations) const; 38 | 39 | Eigen::Vector3d computePIDErrorAcc( 40 | const quadrotor_common::QuadStateEstimate& state_estimate, 41 | const quadrotor_common::TrajectoryPoint& reference_state, 42 | const PositionControllerParams& config) const; 43 | 44 | double computeDesiredCollectiveMassNormalizedThrust( 45 | const Eigen::Quaterniond& attitude_estimate, 46 | const Eigen::Vector3d& desired_acc, 47 | const PositionControllerParams& config) const; 48 | 49 | Eigen::Quaterniond computeDesiredAttitude( 50 | const Eigen::Vector3d& desired_acceleration, const double reference_yaw, 51 | const Eigen::Quaterniond& attitude_estimate) const; 52 | Eigen::Vector3d computeRobustBodyXAxis( 53 | const Eigen::Vector3d& x_B_prototype, const Eigen::Vector3d& x_C, 54 | const Eigen::Vector3d& y_C, 55 | const Eigen::Quaterniond& attitude_estimate) const; 56 | 57 | Eigen::Vector3d computeFeedBackControlBodyrates( 58 | const Eigen::Quaterniond& desired_attitude, 59 | const Eigen::Quaterniond& attitude_estimate, 60 | const PositionControllerParams& config) const; 61 | 62 | bool almostZero(const double value) const; 63 | bool almostZeroThrust(const double thrust_value) const; 64 | 65 | // Constants 66 | static constexpr double kMinNormalizedCollectiveThrust_ = 1.0; 67 | static constexpr double kAlmostZeroValueThreshold_ = 0.001; 68 | static constexpr double kAlmostZeroThrustThreshold_ = 0.01; 69 | 70 | const Eigen::Vector3d kGravity_ = Eigen::Vector3d(0.0, 0.0, -9.81); 71 | }; 72 | 73 | } // namespace position_controller 74 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/src/thrust_mapping.cpp: -------------------------------------------------------------------------------- 1 | #include "sbus_bridge/thrust_mapping.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace thrust_mapping { 7 | 8 | CollectiveThrustMapping::CollectiveThrustMapping() 9 | : thrust_map_a_(0.0), 10 | thrust_map_b_(0.0), 11 | thrust_map_c_(0.0), 12 | perform_thrust_voltage_compensation_(false), 13 | thrust_ratio_voltage_map_a_(0.0), 14 | thrust_ratio_voltage_map_b_(0.0), 15 | n_lipo_cells_(0) {} 16 | 17 | CollectiveThrustMapping::CollectiveThrustMapping( 18 | const double thrust_map_a, const double thrust_map_b, 19 | const double thrust_map_c, const bool perform_thrust_voltage_compensation, 20 | const double thrust_ratio_voltage_map_a, 21 | const double thrust_ratio_voltage_map_b, const int n_lipo_cells) 22 | : thrust_map_a_(thrust_map_a), 23 | thrust_map_b_(thrust_map_b), 24 | thrust_map_c_(thrust_map_c), 25 | perform_thrust_voltage_compensation_(perform_thrust_voltage_compensation), 26 | thrust_ratio_voltage_map_a_(thrust_ratio_voltage_map_a), 27 | thrust_ratio_voltage_map_b_(thrust_ratio_voltage_map_b), 28 | n_lipo_cells_(n_lipo_cells) {} 29 | 30 | CollectiveThrustMapping::~CollectiveThrustMapping() {} 31 | 32 | uint16_t CollectiveThrustMapping::inverseThrustMapping( 33 | const double thrust, const double battery_voltage) const { 34 | double thrust_applied = thrust; 35 | if (perform_thrust_voltage_compensation_) { 36 | if (battery_voltage >= 37 | n_lipo_cells_ * kMinBatteryCompensationVoltagePerCell_ && 38 | battery_voltage <= 39 | n_lipo_cells_ * kMaxBatteryCompensationVoltagePerCell_) { 40 | const double thrust_cmd_voltage_ratio = 41 | thrust_ratio_voltage_map_a_ * battery_voltage + 42 | thrust_ratio_voltage_map_b_; 43 | thrust_applied *= thrust_cmd_voltage_ratio; 44 | } else { 45 | ROS_WARN_THROTTLE(1.0, 46 | "[%s] Battery voltage out of range for compensation", 47 | ros::this_node::getName().c_str()); 48 | } 49 | } 50 | 51 | //Citardauq Formula: Gives a numerically stable solution of the quadratic equation for thrust_map_a ~ 0, which is not the case for the standard formula. 52 | const uint16_t cmd = 2.0 * (thrust_map_c_ - thrust_applied) / (-thrust_map_b_ - sqrt(thrust_map_b_ * thrust_map_b_ - 4.0 * thrust_map_a_ * (thrust_map_c_ - thrust_applied))); 53 | 54 | return cmd; 55 | } 56 | 57 | bool CollectiveThrustMapping::loadParameters() { 58 | ros::NodeHandle pnh("~"); 59 | 60 | #define GET_PARAM(name) \ 61 | if (!quadrotor_common::getParam(#name, name##_, pnh)) return false 62 | 63 | GET_PARAM(thrust_map_a); 64 | GET_PARAM(thrust_map_b); 65 | GET_PARAM(thrust_map_c); 66 | 67 | GET_PARAM(perform_thrust_voltage_compensation); 68 | GET_PARAM(thrust_ratio_voltage_map_a); 69 | GET_PARAM(thrust_ratio_voltage_map_b); 70 | GET_PARAM(n_lipo_cells); 71 | 72 | return true; 73 | 74 | #undef GET_PARAM 75 | } 76 | 77 | } // namespace thrust_mapping 78 | -------------------------------------------------------------------------------- /documents/theory_and_math/img/accelerometer.pdf_tex: -------------------------------------------------------------------------------- 1 | %% Creator: Inkscape inkscape 0.48.3.1, www.inkscape.org 2 | %% PDF/EPS/PS + LaTeX output extension by Johan Engelen, 2010 3 | %% Accompanies image file 'accelerometer.pdf' (pdf, eps, ps) 4 | %% 5 | %% To include the image in your LaTeX document, write 6 | %% \input{.pdf_tex} 7 | %% instead of 8 | %% \includegraphics{.pdf} 9 | %% To scale the image, write 10 | %% \def\svgwidth{} 11 | %% \input{.pdf_tex} 12 | %% instead of 13 | %% \includegraphics[width=]{.pdf} 14 | %% 15 | %% Images with a different path to the parent latex file can 16 | %% be accessed with the `import' package (which may need to be 17 | %% installed) using 18 | %% \usepackage{import} 19 | %% in the preamble, and then including the image with 20 | %% \import{}{.pdf_tex} 21 | %% Alternatively, one can specify 22 | %% \graphicspath{{/}} 23 | %% 24 | %% For more information, please see info/svg-inkscape on CTAN: 25 | %% http://tug.ctan.org/tex-archive/info/svg-inkscape 26 | %% 27 | \begingroup% 28 | \makeatletter% 29 | \providecommand\color[2][]{% 30 | \errmessage{(Inkscape) Color is used for the text in Inkscape, but the package 'color.sty' is not loaded}% 31 | \renewcommand\color[2][]{}% 32 | }% 33 | \providecommand\transparent[1]{% 34 | \errmessage{(Inkscape) Transparency is used (non-zero) for the text in Inkscape, but the package 'transparent.sty' is not loaded}% 35 | \renewcommand\transparent[1]{}% 36 | }% 37 | \providecommand\rotatebox[2]{#2}% 38 | \ifx\svgwidth\undefined% 39 | \setlength{\unitlength}{281.29829102bp}% 40 | \ifx\svgscale\undefined% 41 | \relax% 42 | \else% 43 | \setlength{\unitlength}{\unitlength * \real{\svgscale}}% 44 | \fi% 45 | \else% 46 | \setlength{\unitlength}{\svgwidth}% 47 | \fi% 48 | \global\let\svgwidth\undefined% 49 | \global\let\svgscale\undefined% 50 | \makeatother% 51 | \begin{picture}(1,0.85853336)% 52 | \put(0,0){\includegraphics[width=\unitlength]{accelerometer.pdf}}% 53 | \put(0.41784006,0.13287415){\color[rgb]{1,0,0}\makebox(0,0)[lb]{\smash{$m_M$}}}% 54 | \put(0.32842186,0.60209929){\color[rgb]{0,0,1}\rotatebox{-16.18372667}{\makebox(0,0)[lb]{\smash{Quadrotor}}}}% 55 | \put(0.45003992,0.31444983){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$k_s$}}}% 56 | \put(0.15233209,0.15464341){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$k_s$}}}% 57 | \put(0.3454669,0.23104141){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\bVec{d}$}}}% 58 | \put(0.25784557,0.31148055){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\bVec{f}_s$}}}% 59 | \put(0.25071242,0.37872412){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\bVec{f}_s$}}}% 60 | \put(0.11420426,0.28706156){\color[rgb]{0,0,1}\makebox(0,0)[lb]{\smash{$m_q\cdot\bVec{g}$}}}% 61 | \put(0.25991614,0.83550398){\color[rgb]{0,0,1}\makebox(0,0)[lb]{\smash{$\bVec{f}_{ct}$}}}% 62 | \put(0.28720806,0.00812916){\color[rgb]{1,0,0}\makebox(0,0)[lb]{\smash{$m_M\cdot\bVec{g}$}}}% 63 | \put(0.83384797,0.33877256){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\bVec{g}$}}}% 64 | \end{picture}% 65 | \endgroup% 66 | -------------------------------------------------------------------------------- /gui/rqt_quad_gui/src/rqt_quad_gui/quad_widget_common.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from python_qt_binding import loadUi 3 | qt_version_below_5 = False 4 | try: 5 | # Starting from Qt 5 QWidget is defined in QtWidgets and not QtGui anymore 6 | from python_qt_binding import QtWidgets 7 | from python_qt_binding.QtWidgets import QWidget 8 | except: 9 | from python_qt_binding import QtGui 10 | from python_qt_binding.QtGui import QWidget 11 | qt_version_below_5 = True 12 | from python_qt_binding import QtCore 13 | 14 | from .quad_name_widget import QuadNameWidget 15 | 16 | 17 | class QuadWidgetCommon(QWidget): 18 | def __init__(self): 19 | super(QuadWidgetCommon, self).__init__() 20 | 21 | # the name widget is separate since we need to access it directly 22 | self._name_widget = QuadNameWidget(self) 23 | if qt_version_below_5: 24 | self._column_1 = QtGui.QVBoxLayout() 25 | self._column_2 = QtGui.QVBoxLayout() 26 | else: 27 | self._column_1 = QtWidgets.QVBoxLayout() 28 | self._column_2 = QtWidgets.QVBoxLayout() 29 | 30 | def setup_gui(self, two_columns=True): 31 | if qt_version_below_5: 32 | widget_layout = QtGui.QHBoxLayout() 33 | else: 34 | widget_layout = QtWidgets.QHBoxLayout() 35 | widget_layout.addLayout(self._column_1) 36 | if two_columns: 37 | widget_layout.addLayout(self._column_2) 38 | 39 | if qt_version_below_5: 40 | main_layout = QtGui.QHBoxLayout() 41 | else: 42 | main_layout = QtWidgets.QHBoxLayout() 43 | main_layout = QtWidgets.QVBoxLayout() 44 | main_layout.addLayout(widget_layout) 45 | 46 | self._column_1.setAlignment(QtCore.Qt.AlignTop) 47 | if two_columns: 48 | self._column_2.setAlignment(QtCore.Qt.AlignTop) 49 | widget_layout.setAlignment(QtCore.Qt.AlignTop) 50 | main_layout.setAlignment(QtCore.Qt.AlignTop) 51 | 52 | self.setLayout(main_layout) 53 | 54 | self._update_info_timer = QtCore.QTimer(self) 55 | self._update_info_timer.timeout.connect(self.update_gui) 56 | self._update_info_timer.start(100) 57 | 58 | def get_list_of_plugins(self): 59 | quad_plugins = [] 60 | 61 | for i in range(1, self._column_1.count()): 62 | quad_plugins.append(self._column_1.itemAt(i).widget()) 63 | 64 | for i in range(0, self._column_2.count()): 65 | quad_plugins.append(self._column_2.itemAt(i).widget()) 66 | return quad_plugins 67 | 68 | def connect(self): 69 | quad_name = self._name_widget.getQuadName() 70 | self.setWindowTitle(quad_name) 71 | for plugin in self.get_list_of_plugins(): 72 | plugin.connect(quad_name) 73 | 74 | def disconnect(self): 75 | self.setWindowTitle("RPG Quad Gui") 76 | for plugin in self.get_list_of_plugins(): 77 | plugin.disconnect() 78 | 79 | def update_gui(self): 80 | for plugin in self.get_list_of_plugins(): 81 | plugin.update_gui() 82 | 83 | def getQuadName(self): 84 | return self._name_widget.getQuadName() 85 | 86 | def setQuadName(self, quadname): 87 | self._name_widget.setQuadName(quadname) 88 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/vehicles/components/color_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 45 | 46 | 47 | Gazebo/Black 48 | 49 | 50 | ${update_rate} 51 | 52 | 1.3962634 53 | 54 | 55 | ${image_format} 56 | ${res_x} 57 | ${res_y} 58 | 59 | 60 | 61 | ${min_distance} 62 | ${max_distance} 63 | 64 | 65 | 66 | gaussian 67 | ${noise_mean} 68 | ${noise_stddev} 69 | 70 | 71 | 72 | 73 | 74 | ${name} 75 | true 76 | ${update_rate} 77 | ${ros_topic} 78 | ${cam_info_topic} 79 | ${name}_optical_frame 80 | 0.07 81 | 0.0 82 | 0.0 83 | 0.0 84 | 0.0 85 | 0.0 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /quad_launch_files/launch/real_quad_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: true 47 | ColumnLimit: 80 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 1000000 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Delimiter: pb 97 | Language: TextProto 98 | BasedOnStyle: google 99 | ReflowComments: true 100 | SortIncludes: true 101 | SortUsingDeclarations: true 102 | SpaceAfterCStyleCast: false 103 | SpaceAfterTemplateKeyword: true 104 | SpaceBeforeAssignmentOperators: true 105 | SpaceBeforeParens: ControlStatements 106 | SpaceInEmptyParentheses: false 107 | SpacesBeforeTrailingComments: 2 108 | SpacesInAngles: false 109 | SpacesInContainerLiterals: true 110 | SpacesInCStyleCastParentheses: false 111 | SpacesInParentheses: false 112 | SpacesInSquareBrackets: false 113 | Standard: Auto 114 | TabWidth: 8 115 | UseTab: Never 116 | ... 117 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/src/sbus_msg.cpp: -------------------------------------------------------------------------------- 1 | #include "sbus_bridge/sbus_msg.h" 2 | 3 | #include "sbus_bridge/channel_mapping.h" 4 | 5 | namespace sbus_bridge { 6 | 7 | SBusMsg::SBusMsg() 8 | : timestamp(ros::Time::now()), 9 | digital_channel_1(false), 10 | digital_channel_2(false), 11 | frame_lost(false), 12 | failsafe(false) { 13 | for (int i = 0; i < kNChannels; i++) { 14 | channels[i] = kMeanCmd; 15 | } 16 | } 17 | 18 | SBusMsg::SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg) { 19 | timestamp = sbus_ros_msg.header.stamp; 20 | for (uint8_t i; i < kNChannels; i++) { 21 | channels[i] = sbus_ros_msg.channels[i]; 22 | } 23 | digital_channel_1 = sbus_ros_msg.digital_channel_1; 24 | digital_channel_2 = sbus_ros_msg.digital_channel_2; 25 | frame_lost = sbus_ros_msg.frame_lost; 26 | failsafe = sbus_ros_msg.failsafe; 27 | } 28 | 29 | SBusMsg::~SBusMsg() {} 30 | 31 | sbus_bridge::SbusRosMessage SBusMsg::toRosMessage() const { 32 | sbus_bridge::SbusRosMessage sbus_ros_msg; 33 | sbus_ros_msg.header.stamp = timestamp; 34 | for (uint8_t i; i < kNChannels; i++) { 35 | sbus_ros_msg.channels[i] = channels[i]; 36 | } 37 | sbus_ros_msg.digital_channel_1 = digital_channel_1; 38 | sbus_ros_msg.digital_channel_2 = digital_channel_2; 39 | sbus_ros_msg.frame_lost = frame_lost; 40 | sbus_ros_msg.failsafe = failsafe; 41 | 42 | return sbus_ros_msg; 43 | } 44 | 45 | void SBusMsg::limitAllChannelsFeasible() { 46 | for (uint8_t i = 0; i < kNChannels; i++) { 47 | limitSbusChannelFeasible(i); 48 | } 49 | } 50 | 51 | void SBusMsg::limitSbusChannelFeasible(const int channel_idx) { 52 | if (channel_idx < 0 || channel_idx >= kNChannels) { 53 | return; 54 | } 55 | 56 | if (channels[channel_idx] > kMaxCmd) { 57 | channels[channel_idx] = kMaxCmd; 58 | } 59 | if (channels[channel_idx] < kMinCmd) { 60 | channels[channel_idx] = kMinCmd; 61 | } 62 | } 63 | 64 | void SBusMsg::setThrottleCommand(const uint16_t throttle_cmd) { 65 | channels[channel_mapping::kThrottle] = throttle_cmd; 66 | } 67 | 68 | void SBusMsg::setRollCommand(const uint16_t roll_cmd) { 69 | channels[channel_mapping::kRoll] = roll_cmd; 70 | } 71 | 72 | void SBusMsg::setPitchCommand(const uint16_t pitch_cmd) { 73 | channels[channel_mapping::kPitch] = pitch_cmd; 74 | } 75 | 76 | void SBusMsg::setYawCommand(const uint16_t yaw_cmd) { 77 | channels[channel_mapping::kYaw] = yaw_cmd; 78 | } 79 | 80 | void SBusMsg::setControlMode(const ControlMode& control_mode) { 81 | if (control_mode == ControlMode::ATTITUDE) { 82 | channels[channel_mapping::kControlMode] = kMinCmd; 83 | } else if (control_mode == ControlMode::BODY_RATES) { 84 | channels[channel_mapping::kControlMode] = kMaxCmd; 85 | } 86 | } 87 | 88 | void SBusMsg::setControlModeAttitude() { 89 | setControlMode(ControlMode::ATTITUDE); 90 | } 91 | 92 | void SBusMsg::setControlModeBodyRates() { 93 | setControlMode(ControlMode::BODY_RATES); 94 | } 95 | 96 | void SBusMsg::setArmState(const ArmState& arm_state) { 97 | if (arm_state == ArmState::ARMED) { 98 | channels[channel_mapping::kArming] = kMaxCmd; 99 | } else { 100 | channels[channel_mapping::kArming] = kMinCmd; 101 | } 102 | } 103 | 104 | void SBusMsg::setArmStateArmed() { setArmState(ArmState::ARMED); } 105 | 106 | void SBusMsg::setArmStateDisarmed() { 107 | setArmState(ArmState::DISARMED); 108 | // Should not be necessary but for safety we also set the throttle command 109 | // to the minimum 110 | setThrottleCommand(kMinCmd); 111 | } 112 | 113 | bool SBusMsg::isArmed() const { 114 | if (channels[channel_mapping::kArming] <= kMeanCmd) { 115 | return false; 116 | } 117 | 118 | return true; 119 | } 120 | 121 | ControlMode SBusMsg::getControlMode() const { 122 | if (channels[channel_mapping::kControlMode] > kMeanCmd) { 123 | return ControlMode::BODY_RATES; 124 | } 125 | 126 | return ControlMode::ATTITUDE; 127 | } 128 | 129 | } // namespace sbus_bridge 130 | -------------------------------------------------------------------------------- /control/autopilot/include/autopilot/autopilot_helper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "autopilot/autopilot_states.h" 12 | 13 | namespace autopilot_helper { 14 | 15 | class AutoPilotHelper { 16 | public: 17 | AutoPilotHelper(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 18 | 19 | AutoPilotHelper() 20 | : AutoPilotHelper(ros::NodeHandle(), ros::NodeHandle("~")) {} 21 | 22 | virtual ~AutoPilotHelper(); 23 | 24 | bool feedbackAvailable() const; 25 | double feedbackMessageAge() const; 26 | bool stateEstimateAvailable() const; 27 | 28 | // Blocking function. Blocks until either autopilot feedback is 29 | // available or the timeout has been reached. 30 | bool waitForAutopilotFeedback(const double timeout_s, 31 | const double loop_rate_hz) const; 32 | bool waitForSpecificAutopilotState(const autopilot::States& state, 33 | const double timeout_s, 34 | const double loop_rate_hz) const; 35 | 36 | // Functions to get specific feedback from autopilot 37 | autopilot::States getCurrentAutopilotState() const; 38 | ros::Duration getCurrentControlCommandDelay() const; 39 | ros::Duration getCurrentControlComputationTime() const; 40 | ros::Duration getCurrentTrajectoryExecutionLeftDuration() const; 41 | int getCurrentTrajectoriesLeftInQueue() const; 42 | 43 | quadrotor_common::TrajectoryPoint getCurrentReferenceState() const; 44 | Eigen::Vector3d getCurrentReferencePosition() const; 45 | Eigen::Vector3d getCurrentReferenceVelocity() const; 46 | Eigen::Quaterniond getCurrentReferenceOrientation() const; 47 | double getCurrentReferenceHeading() const; 48 | 49 | quadrotor_common::QuadStateEstimate getCurrentStateEstimate() const; 50 | Eigen::Vector3d getCurrentPositionEstimate() const; 51 | Eigen::Vector3d getCurrentVelocityEstimate() const; 52 | Eigen::Quaterniond getCurrentOrientationEstimate() const; 53 | double getCurrentHeadingEstimate() const; 54 | 55 | // Functions to get specific errors from autopilot 56 | // error = estimate - reference 57 | Eigen::Vector3d getCurrentPositionError() const; 58 | Eigen::Vector3d getCurrentVelocityError() const; 59 | Eigen::Quaterniond getCurrentOrientationError() const; 60 | double getCurrentHeadingError() const; 61 | 62 | // Functions to send commands to the autopilot 63 | void sendPoseCommand(const Eigen::Vector3d& position, 64 | const double heading) const; 65 | void sendVelocityCommand(const Eigen::Vector3d& velocity, 66 | const double heading_rate) const; 67 | void sendReferenceState( 68 | const quadrotor_common::TrajectoryPoint& trajectory_point) const; 69 | void sendTrajectory(const quadrotor_common::Trajectory& trajectory) const; 70 | void sendControlCommandInput( 71 | const quadrotor_common::ControlCommand& control_command) const; 72 | 73 | void sendStart() const; 74 | void sendForceHover() const; 75 | void sendLand() const; 76 | void sendOff() const; 77 | 78 | private: 79 | void autopilotFeedbackCallback( 80 | const quadrotor_msgs::AutopilotFeedback::ConstPtr& msg); 81 | 82 | ros::NodeHandle nh_; 83 | ros::NodeHandle pnh_; 84 | 85 | ros::Publisher pose_pub_; 86 | ros::Publisher velocity_pub_; 87 | ros::Publisher reference_state_pub_; 88 | ros::Publisher trajectory_pub_; 89 | ros::Publisher control_command_input_pub_; 90 | 91 | ros::Publisher start_pub_; 92 | ros::Publisher force_hover_pub_; 93 | ros::Publisher land_pub_; 94 | ros::Publisher off_pub_; 95 | 96 | ros::Subscriber autopilot_feedback_sub_; 97 | 98 | // Variables 99 | quadrotor_msgs::AutopilotFeedback autopilot_feedback_; 100 | bool first_autopilot_feedback_received_; 101 | ros::Time time_last_feedback_received_; 102 | 103 | // Constants 104 | static constexpr double kFeedbackValidTimeout_ = 2.0; 105 | }; 106 | 107 | } // namespace autopilot_helper 108 | -------------------------------------------------------------------------------- /bridges/sbus_bridge/include/sbus_bridge/sbus_bridge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "sbus_bridge/sbus_msg.h" 14 | #include "sbus_bridge/thrust_mapping.h" 15 | 16 | namespace sbus_bridge { 17 | 18 | enum class BridgeState { OFF, ARMING, AUTONOMOUS_FLIGHT, RC_FLIGHT }; 19 | 20 | class SBusBridge : public SBusSerialPort { 21 | public: 22 | SBusBridge(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 23 | 24 | SBusBridge() : SBusBridge(ros::NodeHandle(), ros::NodeHandle("~")) {} 25 | 26 | virtual ~SBusBridge(); 27 | 28 | private: 29 | void watchdogThread(); 30 | 31 | void handleReceivedSbusMessage(const SBusMsg& received_sbus_msg) override; 32 | void controlCommandCallback( 33 | const quadrotor_msgs::ControlCommand::ConstPtr& msg); 34 | void sendSBusMessageToSerialPort(const SBusMsg& sbus_msg); 35 | 36 | SBusMsg generateSBusMessageFromControlCommand( 37 | const quadrotor_msgs::ControlCommand::ConstPtr& control_command) const; 38 | 39 | void setBridgeState(const BridgeState& desired_bridge_state); 40 | 41 | void armBridgeCallback(const std_msgs::Bool::ConstPtr& msg); 42 | void batteryVoltageCallback(const std_msgs::Float32::ConstPtr& msg); 43 | void publishLowLevelFeedback(const ros::TimerEvent& time) const; 44 | 45 | bool loadParameters(); 46 | 47 | ros::NodeHandle nh_; 48 | ros::NodeHandle pnh_; 49 | 50 | // Mutex for: 51 | // - bridge_state_ 52 | // - control_mode_ 53 | // - bridge_armed_ 54 | // - time_last_active_control_command_received_ 55 | // - time_last_rc_msg_received_ 56 | // - arming_counter_ 57 | // - time_last_sbus_msg_sent_ 58 | // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be 59 | // called when "main_mutex_" is locked 60 | mutable std::mutex main_mutex_; 61 | // Mutex for: 62 | // - battery_voltage_ 63 | // - time_last_battery_voltage_received_ 64 | // Also "generateSBusMessageFromControlCommand" should only be called when 65 | // "battery_voltage_mutex_" is locked 66 | mutable std::mutex battery_voltage_mutex_; 67 | 68 | // Publishers 69 | ros::Publisher low_level_feedback_pub_; 70 | ros::Publisher received_sbus_msg_pub_; 71 | 72 | // Subscribers 73 | ros::Subscriber control_command_sub_; 74 | ros::Subscriber arm_bridge_sub_; 75 | ros::Subscriber battery_voltage_sub_; 76 | 77 | // Timer 78 | ros::Timer low_level_feedback_pub_timer_; 79 | 80 | // Watchdog 81 | std::thread watchdog_thread_; 82 | std::atomic_bool stop_watchdog_thread_; 83 | ros::Time time_last_rc_msg_received_; 84 | ros::Time time_last_sbus_msg_sent_; 85 | ros::Time time_last_battery_voltage_received_; 86 | ros::Time time_last_active_control_command_received_; 87 | 88 | BridgeState bridge_state_; 89 | ControlMode control_mode_; 90 | int arming_counter_; 91 | double battery_voltage_; 92 | 93 | // Safety flags 94 | bool bridge_armed_; 95 | bool rc_was_disarmed_once_; 96 | 97 | std::atomic_bool destructor_invoked_; 98 | 99 | thrust_mapping::CollectiveThrustMapping thrust_mapping_; 100 | 101 | // Parameters 102 | std::string port_name_; 103 | bool enable_receiving_sbus_messages_; 104 | 105 | double control_command_timeout_; 106 | double rc_timeout_; 107 | 108 | double mass_; 109 | 110 | bool disable_thrust_mapping_; 111 | 112 | double max_roll_rate_; 113 | double max_pitch_rate_; 114 | double max_yaw_rate_; 115 | 116 | double max_roll_angle_; 117 | double max_pitch_angle_; 118 | 119 | double alpha_vbat_filter_; 120 | bool perform_thrust_voltage_compensation_; 121 | int n_lipo_cells_; 122 | 123 | // Constants 124 | static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; 125 | 126 | static constexpr int kSmoothingFailRepetitions_ = 5; 127 | 128 | static constexpr double kBatteryLowVoltagePerCell_ = 3.6; 129 | static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; 130 | static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; 131 | static constexpr double kBatteryVoltageTimeout_ = 1.0; 132 | }; 133 | 134 | } // namespace sbus_bridge 135 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/include/trajectory_generation_helper/polynomial_trajectory_helper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace trajectory_generation_helper { 10 | 11 | namespace polynomials { 12 | 13 | // Constrained Polynomials 14 | quadrotor_common::Trajectory computeTimeOptimalTrajectory( 15 | const quadrotor_common::TrajectoryPoint& s0, 16 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 17 | const double max_velocity, const double max_normalized_thrust, 18 | const double max_roll_pitch_rate, const double sampling_frequency); 19 | 20 | quadrotor_common::Trajectory computeFixedTimeTrajectory( 21 | const quadrotor_common::TrajectoryPoint& s0, 22 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 23 | const double execution_time, const double sampling_frequency); 24 | 25 | // Minimum Snap Style Polynomials 26 | quadrotor_common::Trajectory generateMinimumSnapTrajectory( 27 | const Eigen::VectorXd& segment_times, 28 | const quadrotor_common::TrajectoryPoint& start_state, 29 | const quadrotor_common::TrajectoryPoint& end_state, 30 | const polynomial_trajectories::PolynomialTrajectorySettings& 31 | trajectory_settings, 32 | const double sampling_frequency); 33 | quadrotor_common::Trajectory generateMinimumSnapTrajectory( 34 | const Eigen::VectorXd& initial_segment_times, 35 | const quadrotor_common::TrajectoryPoint& start_state, 36 | const quadrotor_common::TrajectoryPoint& end_state, 37 | const polynomial_trajectories::PolynomialTrajectorySettings& 38 | trajectory_settings, 39 | const double max_velocity, const double max_normalized_thrust, 40 | const double max_roll_pitch_rate, const double sampling_frequency); 41 | 42 | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 43 | const Eigen::VectorXd& initial_segment_times, 44 | const quadrotor_common::TrajectoryPoint& start_state, 45 | const quadrotor_common::TrajectoryPoint& end_state, 46 | const polynomial_trajectories::PolynomialTrajectorySettings& 47 | trajectory_settings, 48 | const double sampling_frequency); 49 | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 50 | const Eigen::VectorXd& initial_segment_times, 51 | const quadrotor_common::TrajectoryPoint& start_state, 52 | const quadrotor_common::TrajectoryPoint& end_state, 53 | const polynomial_trajectories::PolynomialTrajectorySettings& 54 | trajectory_settings, 55 | const double max_velocity, const double max_normalized_thrust, 56 | const double max_roll_pitch_rate, const double sampling_frequency); 57 | 58 | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( 59 | const Eigen::VectorXd& segment_times, 60 | const polynomial_trajectories::PolynomialTrajectorySettings& 61 | trajectory_settings, 62 | const double sampling_frequency); 63 | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( 64 | const Eigen::VectorXd& initial_segment_times, 65 | const polynomial_trajectories::PolynomialTrajectorySettings& 66 | trajectory_settings, 67 | const double max_velocity, const double max_normalized_thrust, 68 | const double max_roll_pitch_rate, const double sampling_frequency); 69 | 70 | quadrotor_common::Trajectory 71 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 72 | const Eigen::VectorXd& initial_segment_times, 73 | const polynomial_trajectories::PolynomialTrajectorySettings& 74 | trajectory_settings, 75 | const double sampling_frequency); 76 | quadrotor_common::Trajectory 77 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 78 | const Eigen::VectorXd& initial_segment_times, 79 | const polynomial_trajectories::PolynomialTrajectorySettings& 80 | trajectory_settings, 81 | const double max_velocity, const double max_normalized_thrust, 82 | const double max_roll_pitch_rate, const double sampling_frequency); 83 | 84 | // Sampling function 85 | quadrotor_common::Trajectory samplePolynomial( 86 | const polynomial_trajectories::PolynomialTrajectory& polynomial, 87 | const double sampling_frequency); 88 | 89 | } // namespace polynomials 90 | 91 | } // namespace trajectory_generation_helper 92 | -------------------------------------------------------------------------------- /control/position_controller/include/position_controller/position_controller_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace position_controller { 6 | 7 | class PositionControllerParams { 8 | public: 9 | PositionControllerParams() 10 | : use_rate_mode(true), 11 | kpxy(0.0), 12 | kdxy(0.0), 13 | kpz(0.0), 14 | kdz(0.0), 15 | krp(0.0), 16 | kyaw(0.0), 17 | pxy_error_max(0.0), 18 | vxy_error_max(0.0), 19 | pz_error_max(0.0), 20 | vz_error_max(0.0), 21 | yaw_error_max(0.0), 22 | perform_aerodynamics_compensation(false), 23 | k_drag_x(0.0), 24 | k_drag_y(0.0), 25 | k_drag_z(0.0), 26 | k_thrust_horz(0.0) {} 27 | 28 | ~PositionControllerParams() {} 29 | 30 | bool loadParameters(const ros::NodeHandle& pnh) { 31 | const std::string path_rel_to_node = "position_controller"; 32 | 33 | if (!quadrotor_common::getParam(path_rel_to_node + "/use_rate_mode", 34 | use_rate_mode, pnh)) { 35 | return false; 36 | } 37 | 38 | if (!quadrotor_common::getParam(path_rel_to_node + "/kpxy", kpxy, pnh)) { 39 | return false; 40 | } 41 | if (!quadrotor_common::getParam(path_rel_to_node + "/kdxy", kdxy, pnh)) { 42 | return false; 43 | } 44 | 45 | if (!quadrotor_common::getParam(path_rel_to_node + "/kpz", kpz, pnh)) { 46 | return false; 47 | } 48 | if (!quadrotor_common::getParam(path_rel_to_node + "/kdz", kdz, pnh)) { 49 | return false; 50 | } 51 | 52 | if (!quadrotor_common::getParam(path_rel_to_node + "/krp", krp, pnh)) { 53 | return false; 54 | } 55 | if (!quadrotor_common::getParam(path_rel_to_node + "/kyaw", kyaw, pnh)) { 56 | return false; 57 | } 58 | 59 | if (!quadrotor_common::getParam(path_rel_to_node + "/pxy_error_max", 60 | pxy_error_max, pnh)) { 61 | return false; 62 | } 63 | if (!quadrotor_common::getParam(path_rel_to_node + "/vxy_error_max", 64 | vxy_error_max, pnh)) { 65 | return false; 66 | } 67 | if (!quadrotor_common::getParam(path_rel_to_node + "/pz_error_max", 68 | pz_error_max, pnh)) { 69 | return false; 70 | } 71 | if (!quadrotor_common::getParam(path_rel_to_node + "/vz_error_max", 72 | vz_error_max, pnh)) { 73 | return false; 74 | } 75 | if (!quadrotor_common::getParam(path_rel_to_node + "/yaw_error_max", 76 | yaw_error_max, pnh)) { 77 | return false; 78 | } 79 | 80 | if (!quadrotor_common::getParam( 81 | path_rel_to_node + "/perform_aerodynamics_compensation", 82 | perform_aerodynamics_compensation, pnh)) { 83 | return false; 84 | } 85 | if (!quadrotor_common::getParam(path_rel_to_node + "/k_drag_x", k_drag_x, 86 | pnh)) { 87 | return false; 88 | } 89 | if (!quadrotor_common::getParam(path_rel_to_node + "/k_drag_y", k_drag_y, 90 | pnh)) { 91 | return false; 92 | } 93 | if (!quadrotor_common::getParam(path_rel_to_node + "/k_drag_z", k_drag_z, 94 | pnh)) { 95 | return false; 96 | } 97 | if (!quadrotor_common::getParam(path_rel_to_node + "/k_thrust_horz", 98 | k_thrust_horz, pnh)) { 99 | return false; 100 | } 101 | 102 | return true; 103 | } 104 | 105 | // Send bodyrate commands if true, attitude commands otherwise 106 | bool use_rate_mode; 107 | 108 | double kpxy; // [1/s^2] 109 | double kdxy; // [1/s] 110 | 111 | double kpz; // [1/s^2] 112 | double kdz; // [1/s] 113 | 114 | double krp; // [1/s] 115 | double kyaw; // [1/s] 116 | 117 | double pxy_error_max; // [m] 118 | double vxy_error_max; // [m/s] 119 | double pz_error_max; // [m] 120 | double vz_error_max; // [m/s] 121 | double yaw_error_max; // [rad] 122 | 123 | // Whether or not to compensate for aerodynamic effects 124 | bool perform_aerodynamics_compensation; 125 | double k_drag_x; // x-direction rotor drag coefficient 126 | double k_drag_y; // y-direction rotor drag coefficient 127 | double k_drag_z; // z-direction rotor drag coefficient 128 | // thrust correction coefficient due to body horizontal velocity 129 | double k_thrust_horz; 130 | }; 131 | 132 | } // namespace position_controller 133 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RPG Quadrotor Control 2 | 3 | ## License 4 | 5 | The RPG Quadrotor Control repository provides packages that are intended to be used with [ROS](http://www.ros.org/). 6 | This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 7 | 8 | The source code is released under a MIT license. 9 | 10 | ## Instructions 11 | 12 | Instructions for the installation and usage of this software is provided along with further details in our [Wiki](https://github.com/uzh-rpg/rpg_quadrotor_control/wiki). If you have questions or problems with this framework, please use the [issue tracker](https://github.com/uzh-rpg/rpg_quadrotor_control/issues). Please don't send us emails since they might not be answered. If you would like to contribute, please read the [How to Contribute page](https://github.com/uzh-rpg/rpg_quadrotor_control/wiki/How-to-Contribute) first. 13 | 14 | ## Summary 15 | 16 | This repository contains a complete framework for flying quadrotors based on control algorithms developed by the [Robotics and Perception Group](http://www.ifi.uzh.ch/en/rpg.html). 17 | We also provide an interface to the [RotorS](https://github.com/ethz-asl/rotors_simulator) Gazebo plugins to use our algorithms in simulation. 18 | Together with the provided simple trajectory generation library, this can be used to test our sofware entirely in simulation. 19 | We also provide some utility to command a quadrotor with a gamepad through our framework as well as some calibration routines to compensate for varying battery voltage. 20 | Finally, we provide an interface to communicate with flight controllers used for First-Person-View racing. 21 | 22 | The theory behind the included algorithms is summarized in the [theory document](https://github.com/uzh-rpg/rpg_quadrotor_control/blob/master/documents/theory_and_math/theory_and_math.pdf) contained in this repository as well as in our [RA-L18 Paper](http://rpg.ifi.uzh.ch/docs/RAL18_Faessler.pdf) with a technical report attached for further details as well as in our [RA-L17 Paper](http://rpg.ifi.uzh.ch/docs/RAL17_Faessler.pdf). 23 | 24 | This repository makes use of some basic functionalities from the [rpg_quadrotor_common](https://github.com/uzh-rpg/rpg_quadrotor_common) repository and when working with real hardware, the GPIO and ADC functionalities in the [rpg_single_board_io](https://github.com/uzh-rpg/rpg_single_board_io) repository might come in handy. 25 | 26 | #### Publication 27 | 28 | If you use this work in an academic context, please cite the following two RA-L publications: 29 | 30 | M. Faessler, A. Franchi, and D. Scaramuzza, 31 | "**Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories**," 32 | IEEE Robot. Autom. Lett. (RA-L), vol. 3, no. 2, pp. 620–626, Apr. 2018. [[PDF](http://rpg.ifi.uzh.ch/docs/RAL18_Faessler.pdf)] 33 | 34 | @Article{Faessler18ral, 35 | author = {Matthias Faessler and Antonio Franchi and Davide Scaramuzza}, 36 | title = {Differential Flatness of Quadrotor Dynamics Subject to Rotor 37 | Drag for Accurate Tracking of High-Speed Trajectories}, 38 | journal = {{IEEE} Robot. Autom. Lett.}, 39 | year = 2018, 40 | volume = 3, 41 | number = 2, 42 | pages = {620--626}, 43 | month = apr, 44 | doi = {10.1109/LRA.2017.2776353}, 45 | issn = {2377-3766} 46 | } 47 | 48 | M. Faessler, D. Falanga, and D. Scaramuzza, 49 | "**Thrust Mixing, Saturation, and Body-Rate Control for Accurate Aggressive Quadrotor Flight**," 50 | IEEE Robot. Autom. Lett. (RA-L), vol. 2, no. 2, pp. 476–482, Apr. 2017. [[PDF](http://rpg.ifi.uzh.ch/docs/RAL17_Faessler.pdf)] 51 | 52 | @Article{Faessler17ral, 53 | author = {Matthias Faessler and Davide Falanga and Davide Scaramuzza}, 54 | title = {Thrust Mixing, Saturation, and Body-Rate Control for Accurate 55 | Aggressive Quadrotor Flight}, 56 | journal = {{IEEE} Robot. Autom. Lett.}, 57 | year = 2017, 58 | volume = 2, 59 | number = 2, 60 | pages = {476--482}, 61 | month = apr, 62 | doi = {10.1109/LRA.2016.2640362}, 63 | issn = {2377-3766} 64 | } 65 | 66 | Watch the [video](https://youtu.be/VIQILwcM5PA) demonstrating what can be done by the control algorithms in this repository: 67 | 68 | [![Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag](http://rpg.ifi.uzh.ch/img/quad_control/thumb_1.jpeg)](https://youtu.be/VIQILwcM5PA) 69 | 70 | And the [video teaser](https://youtu.be/LmMgx_vKh5s) for our presentation at [ICRA 2018](https://icra2018.org/): 71 | 72 | [![ICRA 2018 Video Teaser: Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag](http://rpg.ifi.uzh.ch/img/quad_control/thumb_2.jpeg)](https://youtu.be/LmMgx_vKh5s) -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/src/circle_trajectory_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory_generation_helper/circle_trajectory_helper.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace trajectory_generation_helper { 7 | 8 | namespace circles { 9 | 10 | quadrotor_common::Trajectory computeHorizontalCircleTrajectory( 11 | const Eigen::Vector3d center, const double radius, const double speed, 12 | const double phi_start, const double phi_end, 13 | const double sampling_frequency) { 14 | /* 15 | * We use a coordinate system with x to the front, y to the left, and z up. 16 | * When setting phi_start = 0.0 the start point will be at 17 | * (radius, 0, 0) + center 18 | * If phi_end > phi_start the trajectory is going counter clock wise 19 | * otherwise it is going clockwise 20 | */ 21 | 22 | quadrotor_common::Trajectory trajectory; 23 | trajectory.trajectory_type = 24 | quadrotor_common::Trajectory::TrajectoryType::GENERAL; 25 | 26 | const double phi_total = phi_end - phi_start; 27 | const double direction = phi_total / fabs(phi_total); 28 | const double omega = direction * fabs(speed / radius); 29 | const double angle_step = fabs(omega / sampling_frequency); 30 | 31 | for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) { 32 | const double phi = phi_start + direction * d_phi; 33 | const double cos_phi = cos(phi); 34 | const double sin_phi = sin(phi); 35 | quadrotor_common::TrajectoryPoint point; 36 | point.time_from_start = ros::Duration(fabs(d_phi / omega)); 37 | point.position = radius * Eigen::Vector3d(cos_phi, sin_phi, 0.0) + center; 38 | point.velocity = radius * omega * Eigen::Vector3d(-sin_phi, cos_phi, 0.0); 39 | point.acceleration = 40 | radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, -sin_phi, 0.0); 41 | point.jerk = 42 | radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, -cos_phi, 0.0); 43 | point.snap = 44 | radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, sin_phi, 0.0); 45 | 46 | trajectory.points.push_back(point); 47 | } 48 | 49 | // Add last point at phi_end 50 | const double phi = phi_start + phi_total; 51 | const double cos_phi = cos(phi); 52 | const double sin_phi = sin(phi); 53 | quadrotor_common::TrajectoryPoint point; 54 | point.time_from_start = ros::Duration(fabs(phi_total / omega)); 55 | point.position = radius * Eigen::Vector3d(cos_phi, sin_phi, 0.0) + center; 56 | point.velocity = radius * omega * Eigen::Vector3d(-sin_phi, cos_phi, 0.0); 57 | point.acceleration = 58 | radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, -sin_phi, 0.0); 59 | point.jerk = 60 | radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, -cos_phi, 0.0); 61 | point.snap = 62 | radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, sin_phi, 0.0); 63 | 64 | trajectory.points.push_back(point); 65 | 66 | return trajectory; 67 | } 68 | 69 | quadrotor_common::Trajectory computeVerticalCircleTrajectory( 70 | const Eigen::Vector3d center, const double orientation, const double radius, 71 | const double speed, const double phi_start, const double phi_end, 72 | const double sampling_frequency) { 73 | /* 74 | * We use a coordinate system with x to the front, y to the left, and z up. 75 | * Orientation is the angle by which the circle is rotated about the z-axis 76 | * If the orientation = 0.0 then the circle is in the x-z plane 77 | * When setting phi_start = 0.0 and orientation = 0.0, the start point will 78 | * be at (radius, 0, 0) + center 79 | * If phi_end > phi_start the circle is going in positive direction around 80 | * the y-axis rotated by orientation. Otherwise it is going in negative 81 | * direction 82 | */ 83 | 84 | quadrotor_common::Trajectory trajectory; 85 | trajectory.trajectory_type = 86 | quadrotor_common::Trajectory::TrajectoryType::GENERAL; 87 | 88 | const double phi_total = phi_end - phi_start; 89 | const double direction = phi_total / fabs(phi_total); 90 | const double omega = direction * fabs(speed / radius); 91 | const double angle_step = fabs(omega / sampling_frequency); 92 | 93 | const Eigen::Quaterniond q_ori = Eigen::Quaterniond( 94 | Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation), 95 | Eigen::Vector3d::UnitZ())); 96 | 97 | for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) { 98 | const double phi = phi_start + direction * d_phi; 99 | const double cos_phi = cos(phi); 100 | const double sin_phi = sin(phi); 101 | quadrotor_common::TrajectoryPoint point; 102 | point.time_from_start = ros::Duration(fabs(d_phi / omega)); 103 | point.position = 104 | q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center; 105 | point.velocity = 106 | q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi)); 107 | point.acceleration = q_ori * (radius * pow(omega, 2.0) * 108 | Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); 109 | point.jerk = q_ori * (radius * pow(omega, 3.0) * 110 | Eigen::Vector3d(sin_phi, 0.0, cos_phi)); 111 | point.snap = q_ori * (radius * pow(omega, 4.0) * 112 | Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); 113 | 114 | trajectory.points.push_back(point); 115 | } 116 | 117 | // Add last point at phi_end 118 | const double phi = phi_start + phi_total; 119 | const double cos_phi = cos(phi); 120 | const double sin_phi = sin(phi); 121 | quadrotor_common::TrajectoryPoint point; 122 | point.time_from_start = ros::Duration(fabs(phi_total / omega)); 123 | point.position = 124 | q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center; 125 | point.velocity = 126 | q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi)); 127 | point.acceleration = q_ori * (radius * pow(omega, 2.0) * 128 | Eigen::Vector3d(-cos_phi, 0.0, sin_phi)); 129 | point.jerk = q_ori * (radius * pow(omega, 3.0) * 130 | Eigen::Vector3d(sin_phi, 0.0, cos_phi)); 131 | point.snap = q_ori * (radius * pow(omega, 4.0) * 132 | Eigen::Vector3d(cos_phi, 0.0, -sin_phi)); 133 | 134 | trajectory.points.push_back(point); 135 | 136 | return trajectory; 137 | } 138 | 139 | } // namespace circles 140 | 141 | } // namespace trajectory_generation_helper 142 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/include/polynomial_trajectories/minimum_snap_trajectories.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "polynomial_trajectories/polynomial_trajectory.h" 9 | #include "polynomial_trajectories/polynomial_trajectory_settings.h" 10 | 11 | namespace polynomial_trajectories { 12 | 13 | namespace minimum_snap_trajectories { 14 | 15 | PolynomialTrajectory generateMinimumSnapTrajectory( 16 | const Eigen::VectorXd& segment_times, 17 | const quadrotor_common::TrajectoryPoint& start_state, 18 | const quadrotor_common::TrajectoryPoint& end_state, 19 | const PolynomialTrajectorySettings& trajectory_settings); 20 | PolynomialTrajectory generateMinimumSnapTrajectory( 21 | const Eigen::VectorXd& initial_segment_times, 22 | const quadrotor_common::TrajectoryPoint& start_state, 23 | const quadrotor_common::TrajectoryPoint& end_state, 24 | const PolynomialTrajectorySettings& trajectory_settings, 25 | const double max_velocity, const double max_normalized_thrust, 26 | const double max_roll_pitch_rate); 27 | 28 | PolynomialTrajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 29 | const Eigen::VectorXd& initial_segment_times, 30 | const quadrotor_common::TrajectoryPoint& start_state, 31 | const quadrotor_common::TrajectoryPoint& end_state, 32 | const PolynomialTrajectorySettings& trajectory_settings); 33 | PolynomialTrajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 34 | const Eigen::VectorXd& initial_segment_times, 35 | const quadrotor_common::TrajectoryPoint& start_state, 36 | const quadrotor_common::TrajectoryPoint& end_state, 37 | const PolynomialTrajectorySettings& trajectory_settings, 38 | const double max_velocity, const double max_normalized_thrust, 39 | const double max_roll_pitch_rate); 40 | 41 | PolynomialTrajectory generateMinimumSnapRingTrajectory( 42 | const Eigen::VectorXd& segment_times, 43 | const PolynomialTrajectorySettings& trajectory_settings); 44 | PolynomialTrajectory generateMinimumSnapRingTrajectory( 45 | const Eigen::VectorXd& initial_segment_times, 46 | const PolynomialTrajectorySettings& trajectory_settings, 47 | const double max_velocity, const double max_normalized_thrust, 48 | const double max_roll_pitch_rate); 49 | 50 | PolynomialTrajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( 51 | const Eigen::VectorXd& initial_segment_times, 52 | const PolynomialTrajectorySettings& trajectory_settings); 53 | PolynomialTrajectory generateMinimumSnapRingTrajectoryWithSegmentRefinement( 54 | const Eigen::VectorXd& initial_segment_times, 55 | const PolynomialTrajectorySettings& trajectory_settings, 56 | const double max_velocity, const double max_normalized_thrust, 57 | const double max_roll_pitch_rate); 58 | 59 | // these functions should not be used from the outside 60 | namespace implementation { 61 | Eigen::MatrixXd generate1DTrajectory(const int num_polynoms, 62 | const int polynomial_order, 63 | const Eigen::MatrixXd& H, 64 | const Eigen::VectorXd& f, 65 | const Eigen::MatrixXd& A, 66 | const Eigen::VectorXd& b, 67 | double* optimization_cost); 68 | 69 | Eigen::MatrixXd generateHMatrix( 70 | const PolynomialTrajectorySettings& trajectory_settings, 71 | const int num_polynoms, const Eigen::VectorXd& tau_dot); 72 | Eigen::VectorXd generateFVector( 73 | const PolynomialTrajectorySettings& trajectory_settings, 74 | const Eigen::VectorXd& way_points_1D, const int num_polynoms); 75 | Eigen::MatrixXd generateEqualityConstraintsAMatrix( 76 | const PolynomialTrajectorySettings& trajectory_settings, 77 | const int num_polynoms, const Eigen::VectorXd& tau_dot); 78 | Eigen::VectorXd generateEqualityConstraintsBVector( 79 | const PolynomialTrajectorySettings& trajectory_settings, 80 | const int num_polynoms, const Eigen::VectorXd& way_points_1D, 81 | const Eigen::Vector3d& start_conditions, 82 | const Eigen::Vector3d& end_conditions); 83 | 84 | Eigen::MatrixXd generateRingEqualityConstraintsAMatrix( 85 | const PolynomialTrajectorySettings& trajectory_settings, 86 | const int num_polynoms, const Eigen::VectorXd& tau_dot); 87 | Eigen::VectorXd generateRingEqualityConstraintsBVector( 88 | const PolynomialTrajectorySettings& trajectory_settings, 89 | const int num_polynoms, const Eigen::VectorXd& way_points_1D); 90 | 91 | Eigen::VectorXd computeCostGradient( 92 | const PolynomialTrajectory& initial_trajectory, 93 | const PolynomialTrajectorySettings& trajectory_settings); 94 | Eigen::VectorXd computeSearchDirection( 95 | const PolynomialTrajectory& initial_trajectory, 96 | const Eigen::VectorXd& gradient); 97 | Eigen::VectorXd updateSegmentTimes( 98 | const PolynomialTrajectory& initial_trajectory, 99 | const Eigen::VectorXd& gradient, 100 | const PolynomialTrajectorySettings& trajectory_settings); 101 | 102 | PolynomialTrajectory enforceMaximumVelocityAndThrust( 103 | const PolynomialTrajectory& initial_trajectory, 104 | const PolynomialTrajectorySettings& trajectory_settings, 105 | const double max_velocity, const double max_normalized_thrust, 106 | const double max_roll_pitch_rate); 107 | 108 | bool computeMaximaGradient( 109 | const PolynomialTrajectory& trajectory, const Eigen::Vector3d& maxima, 110 | const PolynomialTrajectorySettings& trajectory_settings, 111 | Eigen::Vector3d* gradient); 112 | 113 | std::vector reorganiceCoefficientsSegmentWise( 114 | const std::vector& coefficients, const int num_segments, 115 | const int polynomial_order); 116 | PolynomialTrajectorySettings ensureFeasibleTrajectorySettings( 117 | const PolynomialTrajectorySettings& original_trajectory_settings, 118 | const int min_poly_order); 119 | std::vector addStartAndEndToWayPointList( 120 | const std::vector& intermediate_way_points, 121 | const Eigen::Vector3d& start_position, const Eigen::Vector3d& end_position); 122 | 123 | Eigen::VectorXd solveQuadraticProgram(const Eigen::MatrixXd& H, 124 | const Eigen::VectorXd& f, 125 | const Eigen::MatrixXd& A_eq, 126 | const Eigen::VectorXd& b_eq, 127 | double* objective_value); 128 | } // namespace implementation 129 | 130 | } // namespace minimum_snap_trajectories 131 | 132 | } // namespace polynomial_trajectories 133 | -------------------------------------------------------------------------------- /simulation/rpg_rotors_interface/launch/basics/base_quad_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 142 | 143 | 144 | 145 | 148 | 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /utils/vbat_thrust_calibration/scripts/vbat_thrust_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from quadrotor_msgs.msg import AutopilotFeedback 7 | from quadrotor_msgs.msg import ControlCommand 8 | from quadrotor_msgs.msg import LowLevelFeedback 9 | import rospy 10 | 11 | 12 | class VbatThrustCalibration: 13 | 14 | def __init__(self): 15 | 16 | # Load parameters 17 | self.time_average_interval = float( 18 | rospy.get_param('~time_average_interval', 1.0)) 19 | self.vbat_min = float( 20 | rospy.get_param('~min_battery_voltage', 10.7)) 21 | self.vbat_max = float( 22 | rospy.get_param('~max_battery_voltage', 12.2)) 23 | self.hover_tolerance_radius = float( 24 | rospy.get_param('~hover_tolerance_radius', 0.3)) 25 | 26 | self.min_n_samples = 10 27 | 28 | # store measurements of vbat and commanded thrusts 29 | self.vbat_meas = np.array([]) 30 | self.thrust_cmds = np.array([]) 31 | 32 | # We solve A x = B such that || B - A x ||^2 is minimized 33 | self.A = np.empty([0, 2]) 34 | self.B = np.empty([0, 1]) 35 | 36 | # Store min and max battery voltages used for identification 37 | self.id_max_voltage = self.vbat_max 38 | self.id_min_voltage = self.vbat_min 39 | 40 | # Some flags 41 | self.received_first_low_level_feedback = False 42 | self.received_first_state_estimate = False 43 | self.received_first_autopilot_feedback = False 44 | self.collecting_data = False 45 | self.vbat_in_range = False 46 | 47 | rospy.Subscriber( 48 | "low_level_feedback", LowLevelFeedback, self.low_level_feedback_cb) 49 | rospy.Subscriber( 50 | "control_command", ControlCommand, self.control_command_cb) 51 | rospy.Subscriber( 52 | "state_estimate", Odometry, self.state_estimate_cb) 53 | rospy.Subscriber( 54 | "autopilot/feedback", AutopilotFeedback, self.autopilot_fb_cb) 55 | 56 | def low_level_feedback_cb(self, msg): 57 | 58 | if not self.received_first_low_level_feedback: 59 | if msg.battery_voltage < self.vbat_max: 60 | rospy.logwarn( 61 | "[{0}] Battery voltage was below maximum of estimation " 62 | "interval ({1} < {2}) when starting the " 63 | "calibration, it will proceed anyway".format( 64 | rospy.get_name(), msg.battery_voltage, self.vbat_max)) 65 | self.id_max_voltage = msg.battery_voltage 66 | self.vbat_in_range = True 67 | rospy.loginfo( 68 | "[{0}] Received first low level feedback message".format( 69 | rospy.get_name())) 70 | self.received_first_low_level_feedback = True 71 | 72 | if msg.battery_voltage <= self.vbat_max and not self.vbat_in_range: 73 | rospy.loginfo( 74 | "[{0}] Battery voltage dropped below max capturing " 75 | "threshold, starting calibration".format(rospy.get_name())) 76 | self.id_max_voltage = self.vbat_max 77 | self.vbat_in_range = True 78 | 79 | # Check if battery is low 80 | if msg.battery_voltage < self.vbat_min: 81 | rospy.loginfo( 82 | "[{0}] Stopping because of low battery voltage ({1}V)".format( 83 | rospy.get_name(), msg.battery_voltage)) 84 | 85 | self.id_min_voltage = self.vbat_min 86 | self.compute_coefficients() 87 | 88 | rospy.signal_shutdown("Stopped because of low battery") 89 | 90 | # Store vbat measurements 91 | if self.collecting_data: 92 | self.vbat_meas = np.append( 93 | self.vbat_meas, np.array([msg.battery_voltage]), axis=0) 94 | 95 | def control_command_cb(self, msg): 96 | 97 | if (self.received_first_low_level_feedback and 98 | self.received_first_state_estimate and 99 | self.received_first_autopilot_feedback and 100 | self.vbat_in_range and not self.collecting_data): 101 | rospy.loginfo( 102 | "[{0}] Starting to collect data now".format( 103 | rospy.get_name())) 104 | self.collecting_data = True 105 | self.t_started_average_interval = rospy.get_time() 106 | 107 | if self.collecting_data: 108 | self.thrust_cmds = np.append( 109 | self.thrust_cmds, np.array([msg.collective_thrust]), axis=0) 110 | 111 | # Check if a time interval has passed 112 | if (self.collecting_data and 113 | (rospy.get_time() - self.t_started_average_interval) >= 114 | self.time_average_interval): 115 | # Store averaged measurements 116 | if self.vbat_meas.shape[0] > 0 and self.thrust_cmds.shape[0] > 0: 117 | # Avoid considering intervals where no messages were received 118 | self.A = np.append( 119 | self.A, [[np.mean(self.vbat_meas), 1]], axis=0) 120 | self.B = np.append( 121 | self.B, [[np.mean(self.thrust_cmds) / 9.81]], axis=0) 122 | # Reset values 123 | self.vbat_meas = np.array([]) 124 | self.thrust_cmds = np.array([]) 125 | self.t_started_average_interval = rospy.get_time() 126 | 127 | def state_estimate_cb(self, msg): 128 | 129 | if not self.received_first_state_estimate: 130 | self.hover_position = np.array( 131 | [msg.pose.pose.position.x, msg.pose.pose.position.y, 132 | msg.pose.pose.position.z]) 133 | rospy.loginfo( 134 | "[{0}] Received first state estimate message".format( 135 | rospy.get_name())) 136 | self.received_first_state_estimate = True 137 | 138 | # Check if we are hovering well 139 | hover_deviation = np.linalg.norm( 140 | self.hover_position - np.array( 141 | [msg.pose.pose.position.x, msg.pose.pose.position.y, 142 | msg.pose.pose.position.z])) 143 | if hover_deviation > self.hover_tolerance_radius: 144 | rospy.logerr( 145 | "[{0}] Too large deviation from hover position, aborting " 146 | "calibration, consider redoing this calibration instead of " 147 | "using the computed coefficients".format(rospy.get_name())) 148 | self.id_min_voltage = self.A[-1][0] 149 | self.compute_coefficients() 150 | rospy.signal_shutdown("Too large hover deviation") 151 | 152 | def autopilot_fb_cb(self, msg): 153 | 154 | if not self.received_first_autopilot_feedback: 155 | rospy.loginfo( 156 | "[{0}] Received first autopilot feedback message".format( 157 | rospy.get_name())) 158 | self.received_first_autopilot_feedback = True 159 | 160 | if msg.autopilot_state != msg.HOVER: 161 | rospy.logerr( 162 | "[{0}] Autopilot is not in hover, stopping vbat thrust " 163 | "calibration.".format(rospy.get_name())) 164 | rospy.signal_shutdown("Autopilot not in hover") 165 | 166 | def compute_coefficients(self): 167 | 168 | if self.A.shape[0] < self.min_n_samples: 169 | rospy.logerr( 170 | "[{0}] Too few samples ({1} < {2}) to compute vbat thrust " 171 | "calibration".format( 172 | rospy.get_name(), self.A.shape[0], self.min_n_samples)) 173 | return 174 | 175 | sol = np.linalg.lstsq(self.A, self.B) 176 | if sol[2] < 2: # Check rank of A 177 | rospy.logerr( 178 | "[{0}] Could not compute coefficients because A matrix " 179 | "does not have full rank".format(rospy.get_name())) 180 | else: 181 | rospy.loginfo( 182 | "[{0}] Calibration was performed for battery voltages between " 183 | "{1}V and {2}V:".format(rospy.get_name(), self.id_max_voltage, 184 | self.id_min_voltage)) 185 | rospy.loginfo( 186 | "[{0}] Vbat Thrust calibration coefficients:".format( 187 | rospy.get_name())) 188 | rospy.loginfo( 189 | "[{0}] thrust_ratio_voltage_map_a: {1}".format( 190 | rospy.get_name(), sol[0][0])) 191 | rospy.loginfo( 192 | "[{0}] thrust_ratio_voltage_map_b: {1}".format( 193 | rospy.get_name(), sol[0][1])) 194 | 195 | 196 | if __name__ == '__main__': 197 | 198 | try: 199 | rospy.init_node("vbat_thrust_calibration", anonymous=True) 200 | 201 | vbat_thrust_calibration = VbatThrustCalibration() 202 | rospy.spin() 203 | 204 | except rospy.ROSInterruptException: 205 | pass 206 | -------------------------------------------------------------------------------- /trajectory_planning/trajectory_generation_helper/src/polynomial_trajectory_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "trajectory_generation_helper/polynomial_trajectory_helper.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace trajectory_generation_helper { 8 | 9 | namespace polynomials { 10 | 11 | // Constrained Polynomials 12 | quadrotor_common::Trajectory computeTimeOptimalTrajectory( 13 | const quadrotor_common::TrajectoryPoint& s0, 14 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 15 | const double max_velocity, const double max_normalized_thrust, 16 | const double max_roll_pitch_rate, const double sampling_frequency) { 17 | polynomial_trajectories::PolynomialTrajectory polynomial = 18 | polynomial_trajectories::constrained_polynomial_trajectories:: 19 | computeTimeOptimalTrajectory(s0, s1, order_of_continuity, 20 | max_velocity, max_normalized_thrust, 21 | max_roll_pitch_rate); 22 | 23 | return samplePolynomial(polynomial, sampling_frequency); 24 | } 25 | 26 | quadrotor_common::Trajectory computeFixedTimeTrajectory( 27 | const quadrotor_common::TrajectoryPoint& s0, 28 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 29 | const double execution_time, const double sampling_frequency) { 30 | polynomial_trajectories::PolynomialTrajectory polynomial = 31 | polynomial_trajectories::constrained_polynomial_trajectories:: 32 | computeFixedTimeTrajectory(s0, s1, order_of_continuity, 33 | execution_time); 34 | 35 | return samplePolynomial(polynomial, sampling_frequency); 36 | } 37 | 38 | // Minimum Snap Style Polynomials 39 | quadrotor_common::Trajectory generateMinimumSnapTrajectory( 40 | const Eigen::VectorXd& segment_times, 41 | const quadrotor_common::TrajectoryPoint& start_state, 42 | const quadrotor_common::TrajectoryPoint& end_state, 43 | const polynomial_trajectories::PolynomialTrajectorySettings& 44 | trajectory_settings, 45 | const double sampling_frequency) { 46 | polynomial_trajectories::PolynomialTrajectory polynomial = 47 | polynomial_trajectories::minimum_snap_trajectories:: 48 | generateMinimumSnapTrajectory(segment_times, start_state, end_state, 49 | trajectory_settings); 50 | 51 | return samplePolynomial(polynomial, sampling_frequency); 52 | } 53 | 54 | quadrotor_common::Trajectory generateMinimumSnapTrajectory( 55 | const Eigen::VectorXd& initial_segment_times, 56 | const quadrotor_common::TrajectoryPoint& start_state, 57 | const quadrotor_common::TrajectoryPoint& end_state, 58 | const polynomial_trajectories::PolynomialTrajectorySettings& 59 | trajectory_settings, 60 | const double max_velocity, const double max_normalized_thrust, 61 | const double max_roll_pitch_rate, const double sampling_frequency) { 62 | polynomial_trajectories::PolynomialTrajectory polynomial = 63 | polynomial_trajectories::minimum_snap_trajectories:: 64 | generateMinimumSnapTrajectory(initial_segment_times, start_state, 65 | end_state, trajectory_settings, 66 | max_velocity, max_normalized_thrust, 67 | max_roll_pitch_rate); 68 | 69 | return samplePolynomial(polynomial, sampling_frequency); 70 | } 71 | 72 | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 73 | const Eigen::VectorXd& initial_segment_times, 74 | const quadrotor_common::TrajectoryPoint& start_state, 75 | const quadrotor_common::TrajectoryPoint& end_state, 76 | const polynomial_trajectories::PolynomialTrajectorySettings& 77 | trajectory_settings, 78 | const double sampling_frequency) { 79 | polynomial_trajectories::PolynomialTrajectory polynomial = 80 | polynomial_trajectories::minimum_snap_trajectories:: 81 | generateMinimumSnapTrajectoryWithSegmentRefinement( 82 | initial_segment_times, start_state, end_state, 83 | trajectory_settings); 84 | 85 | return samplePolynomial(polynomial, sampling_frequency); 86 | } 87 | 88 | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement( 89 | const Eigen::VectorXd& initial_segment_times, 90 | const quadrotor_common::TrajectoryPoint& start_state, 91 | const quadrotor_common::TrajectoryPoint& end_state, 92 | const polynomial_trajectories::PolynomialTrajectorySettings& 93 | trajectory_settings, 94 | const double max_velocity, const double max_normalized_thrust, 95 | const double max_roll_pitch_rate, const double sampling_frequency) { 96 | polynomial_trajectories::PolynomialTrajectory polynomial = 97 | polynomial_trajectories::minimum_snap_trajectories:: 98 | generateMinimumSnapTrajectoryWithSegmentRefinement( 99 | initial_segment_times, start_state, end_state, 100 | trajectory_settings, max_velocity, max_normalized_thrust, 101 | max_roll_pitch_rate); 102 | 103 | return samplePolynomial(polynomial, sampling_frequency); 104 | } 105 | 106 | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( 107 | const Eigen::VectorXd& segment_times, 108 | const polynomial_trajectories::PolynomialTrajectorySettings& 109 | trajectory_settings, 110 | const double sampling_frequency) { 111 | polynomial_trajectories::PolynomialTrajectory polynomial = 112 | polynomial_trajectories::minimum_snap_trajectories:: 113 | generateMinimumSnapRingTrajectory(segment_times, trajectory_settings); 114 | 115 | return samplePolynomial(polynomial, sampling_frequency); 116 | } 117 | 118 | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory( 119 | const Eigen::VectorXd& initial_segment_times, 120 | const polynomial_trajectories::PolynomialTrajectorySettings& 121 | trajectory_settings, 122 | const double max_velocity, const double max_normalized_thrust, 123 | const double max_roll_pitch_rate, const double sampling_frequency) { 124 | polynomial_trajectories::PolynomialTrajectory polynomial = 125 | polynomial_trajectories::minimum_snap_trajectories:: 126 | generateMinimumSnapRingTrajectory( 127 | initial_segment_times, trajectory_settings, max_velocity, 128 | max_normalized_thrust, max_roll_pitch_rate); 129 | 130 | return samplePolynomial(polynomial, sampling_frequency); 131 | } 132 | 133 | quadrotor_common::Trajectory 134 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 135 | const Eigen::VectorXd& initial_segment_times, 136 | const polynomial_trajectories::PolynomialTrajectorySettings& 137 | trajectory_settings, 138 | const double sampling_frequency) { 139 | polynomial_trajectories::PolynomialTrajectory polynomial = 140 | polynomial_trajectories::minimum_snap_trajectories:: 141 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 142 | initial_segment_times, trajectory_settings); 143 | 144 | return samplePolynomial(polynomial, sampling_frequency); 145 | } 146 | 147 | quadrotor_common::Trajectory 148 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 149 | const Eigen::VectorXd& initial_segment_times, 150 | const polynomial_trajectories::PolynomialTrajectorySettings& 151 | trajectory_settings, 152 | const double max_velocity, const double max_normalized_thrust, 153 | const double max_roll_pitch_rate, const double sampling_frequency) { 154 | polynomial_trajectories::PolynomialTrajectory polynomial = 155 | polynomial_trajectories::minimum_snap_trajectories:: 156 | generateMinimumSnapRingTrajectoryWithSegmentRefinement( 157 | initial_segment_times, trajectory_settings, max_velocity, 158 | max_normalized_thrust, max_roll_pitch_rate); 159 | 160 | return samplePolynomial(polynomial, sampling_frequency); 161 | } 162 | 163 | // Sampling function 164 | quadrotor_common::Trajectory samplePolynomial( 165 | const polynomial_trajectories::PolynomialTrajectory& polynomial, 166 | const double sampling_frequency) { 167 | if (polynomial.trajectory_type == 168 | polynomial_trajectories::TrajectoryType::UNDEFINED) { 169 | return quadrotor_common::Trajectory(); 170 | } 171 | 172 | quadrotor_common::Trajectory trajectory; 173 | 174 | trajectory.points.push_back(polynomial.start_state); 175 | 176 | const ros::Duration dt(1.0 / sampling_frequency); 177 | ros::Duration time_from_start = polynomial.start_state.time_from_start + dt; 178 | 179 | while (time_from_start < polynomial.T) { 180 | trajectory.points.push_back(polynomial_trajectories::getPointFromTrajectory( 181 | polynomial, time_from_start)); 182 | time_from_start += dt; 183 | } 184 | 185 | trajectory.points.push_back(polynomial.end_state); 186 | 187 | trajectory.trajectory_type = 188 | quadrotor_common::Trajectory::TrajectoryType::GENERAL; 189 | 190 | return trajectory; 191 | } 192 | 193 | } // namespace polynomials 194 | 195 | } // namespace trajectory_generation_helper 196 | -------------------------------------------------------------------------------- /control/autopilot/include/autopilot/autopilot.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "autopilot/autopilot_states.h" 26 | 27 | namespace autopilot { 28 | 29 | template 30 | class AutoPilot { 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | AutoPilot(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); 35 | 36 | AutoPilot() : AutoPilot(ros::NodeHandle(), ros::NodeHandle("~")) {} 37 | 38 | virtual ~AutoPilot(); 39 | 40 | private: 41 | void watchdogThread(); 42 | void goToPoseThread(); 43 | 44 | void stateEstimateCallback(const nav_msgs::Odometry::ConstPtr& msg); 45 | void lowLevelFeedbackCallback( 46 | const quadrotor_msgs::LowLevelFeedback::ConstPtr& msg); 47 | 48 | void poseCommandCallback(const geometry_msgs::PoseStamped::ConstPtr& msg); 49 | void velocityCommandCallback( 50 | const geometry_msgs::TwistStamped::ConstPtr& msg); 51 | void referenceStateCallback( 52 | const quadrotor_msgs::TrajectoryPoint::ConstPtr& msg); 53 | void trajectoryCallback(const quadrotor_msgs::Trajectory::ConstPtr& msg); 54 | void controlCommandInputCallback( 55 | const quadrotor_msgs::ControlCommand::ConstPtr& msg); 56 | void startCallback(const std_msgs::Empty::ConstPtr& msg); 57 | void forceHoverCallback(const std_msgs::Empty::ConstPtr& msg); 58 | void landCallback(const std_msgs::Empty::ConstPtr& msg); 59 | void offCallback(const std_msgs::Empty::ConstPtr& msg); 60 | 61 | quadrotor_common::ControlCommand start( 62 | const quadrotor_common::QuadStateEstimate& state_estimate); 63 | quadrotor_common::ControlCommand hover( 64 | const quadrotor_common::QuadStateEstimate& state_estimate); 65 | quadrotor_common::ControlCommand land( 66 | const quadrotor_common::QuadStateEstimate& state_estimate); 67 | quadrotor_common::ControlCommand breakVelocity( 68 | const quadrotor_common::QuadStateEstimate& state_estimate); 69 | quadrotor_common::ControlCommand waitForGoToPoseAction( 70 | const quadrotor_common::QuadStateEstimate& state_estimate); 71 | quadrotor_common::ControlCommand velocityControl( 72 | const quadrotor_common::QuadStateEstimate& state_estimate); 73 | quadrotor_common::ControlCommand followReference( 74 | const quadrotor_common::QuadStateEstimate& state_estimate); 75 | quadrotor_common::ControlCommand executeTrajectory( 76 | const quadrotor_common::QuadStateEstimate& state_estimate, 77 | ros::Duration* trajectory_execution_left_duration, 78 | int* trajectories_left_in_queue); 79 | 80 | void setAutoPilotState(const States& new_state); 81 | void setAutoPilotStateForced(const States& new_state); 82 | double timeInCurrentState() const; 83 | quadrotor_common::QuadStateEstimate getPredictedStateEstimate( 84 | const ros::Time& time) const; 85 | 86 | void publishControlCommand(const quadrotor_common::ControlCommand& command); 87 | void publishAutopilotFeedback( 88 | const States& autopilot_state, const ros::Duration& control_command_delay, 89 | const ros::Duration& control_computation_time, 90 | const ros::Duration& trajectory_execution_left_duration, 91 | const int trajectories_left_in_queue, 92 | const quadrotor_msgs::LowLevelFeedback& low_level_feedback, 93 | const quadrotor_common::TrajectoryPoint& reference_state, 94 | const quadrotor_common::QuadStateEstimate& state_estimate); 95 | 96 | bool loadParameters(); 97 | 98 | ros::NodeHandle nh_; 99 | ros::NodeHandle pnh_; 100 | 101 | // Main mutex: 102 | // This mutex is locked in the watchdogThread and all the Callback functions 103 | // All other functions should only be called from one of those and therefore 104 | // do not need to lock the mutex themselves 105 | // The functions that lock the mutex do so at their start of execution and 106 | // keep it locked until they are finished 107 | mutable std::mutex main_mutex_; 108 | 109 | // Go to pose mutex: 110 | // This mutex is locked in the goToPoseThread and the poseCommandCallback and 111 | // should be called according to the order in goToPoseThread before locking 112 | // the main thread 113 | // It specifically protects 114 | // - requested_go_to_pose_ 115 | // - received_go_to_pose_command_ 116 | mutable std::mutex go_to_pose_mutex_; 117 | 118 | ros::Publisher control_command_pub_; 119 | ros::Publisher autopilot_feedback_pub_; 120 | 121 | ros::Subscriber state_estimate_sub_; 122 | ros::Subscriber low_level_feedback_sub_; 123 | ros::Subscriber pose_command_sub_; 124 | ros::Subscriber velocity_command_sub_; 125 | ros::Subscriber reference_state_sub_; 126 | ros::Subscriber trajectory_sub_; 127 | ros::Subscriber control_command_input_sub_; 128 | ros::Subscriber start_sub_; 129 | ros::Subscriber force_hover_sub_; 130 | ros::Subscriber land_sub_; 131 | ros::Subscriber off_sub_; 132 | 133 | state_predictor::StatePredictor state_predictor_; 134 | 135 | Tcontroller base_controller_; 136 | Tparams base_controller_params_; 137 | 138 | quadrotor_common::TrajectoryPoint reference_state_; 139 | quadrotor_common::Trajectory reference_trajectory_; 140 | 141 | // Values received from callbacks 142 | quadrotor_common::QuadStateEstimate received_state_est_; 143 | geometry_msgs::TwistStamped desired_velocity_command_; 144 | quadrotor_msgs::TrajectoryPoint reference_state_input_; 145 | quadrotor_msgs::LowLevelFeedback received_low_level_feedback_; 146 | 147 | States autopilot_state_; 148 | States state_before_rc_manual_flight_; 149 | 150 | // State switching variables 151 | bool state_estimate_available_; 152 | ros::Time time_of_switch_to_current_state_; 153 | bool first_time_in_new_state_; 154 | Eigen::Vector3d initial_start_position_; 155 | Eigen::Vector3d initial_land_position_; 156 | bool time_to_ramp_down_; 157 | ros::Time time_started_ramping_down_; 158 | double initial_drop_thrust_; 159 | ros::Time time_last_velocity_command_handled_; 160 | ros::Time time_last_reference_state_input_received_; 161 | States desired_state_after_breaking_; 162 | States state_before_emergency_landing_; 163 | bool force_breaking_; 164 | 165 | // Go to pose variables 166 | std::thread go_to_pose_thread_; 167 | geometry_msgs::PoseStamped requested_go_to_pose_; 168 | bool received_go_to_pose_command_; 169 | std::atomic_bool stop_go_to_pose_thread_; 170 | 171 | // Trajectory execution variables 172 | std::list trajectory_queue_; 173 | ros::Time time_start_trajectory_execution_; 174 | 175 | // Control command input variables 176 | ros::Time time_last_control_command_input_received_; 177 | bool last_control_command_input_thrust_high_; 178 | 179 | // Watchdog 180 | std::thread watchdog_thread_; 181 | std::atomic_bool stop_watchdog_thread_; 182 | ros::Time time_last_state_estimate_received_; 183 | ros::Time time_started_emergency_landing_; 184 | 185 | std::atomic_bool destructor_invoked_; 186 | 187 | ros::Time time_last_autopilot_feedback_published_; 188 | 189 | // Parameters 190 | double state_estimate_timeout_; 191 | bool velocity_estimate_in_world_frame_; 192 | double control_command_delay_; 193 | double start_land_velocity_; 194 | double start_land_acceleration_; 195 | double start_idle_duration_; 196 | double idle_thrust_; 197 | double optitrack_start_height_; 198 | double optitrack_start_land_timeout_; 199 | double optitrack_land_drop_height_; 200 | double propeller_ramp_down_timeout_; 201 | double breaking_velocity_threshold_; 202 | double breaking_timeout_; 203 | double go_to_pose_max_velocity_; 204 | double go_to_pose_max_normalized_thrust_; 205 | double go_to_pose_max_roll_pitch_rate_; 206 | double velocity_command_input_timeout_; 207 | double tau_velocity_command_; 208 | double reference_state_input_timeout_; 209 | double emergency_land_duration_; 210 | double emergency_land_thrust_; 211 | double control_command_input_timeout_; 212 | bool enable_command_feedthrough_; 213 | double predictive_control_lookahead_; 214 | 215 | // Constants 216 | static constexpr double kVelocityCommandZeroThreshold_ = 0.03; 217 | static constexpr double kPositionJumpTolerance_ = 0.5; 218 | static constexpr double kGravityAcc_ = 9.81; 219 | static constexpr double kWatchdogFrequency_ = 50.0; 220 | static constexpr double kMaxAutopilotFeedbackPublishFrequency_ = 60.0; 221 | static constexpr double kGoToPoseIdleFrequency_ = 50.0; 222 | static constexpr double kGoToPoseTrajectorySamplingFrequency_ = 50.0; 223 | static constexpr int kGoToPosePolynomialOrderOfContinuity_ = 5; 224 | static constexpr double kGoToPoseNeglectThreshold_ = 0.05; 225 | static constexpr double kThrustHighThreshold_ = 0.5; 226 | }; 227 | 228 | } // namespace autopilot 229 | 230 | #include "./autopilot_inl.h" 231 | -------------------------------------------------------------------------------- /control/autopilot/src/autopilot_helper.cpp: -------------------------------------------------------------------------------- 1 | #include "autopilot/autopilot_helper.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace autopilot_helper { 13 | 14 | AutoPilotHelper::AutoPilotHelper(const ros::NodeHandle& nh, 15 | const ros::NodeHandle& pnh) 16 | : autopilot_feedback_(), 17 | first_autopilot_feedback_received_(false), 18 | time_last_feedback_received_() { 19 | pose_pub_ = 20 | nh_.advertise("autopilot/pose_command", 1); 21 | velocity_pub_ = nh_.advertise( 22 | "autopilot/velocity_command", 1); 23 | reference_state_pub_ = nh_.advertise( 24 | "autopilot/reference_state", 1); 25 | trajectory_pub_ = 26 | nh_.advertise("autopilot/trajectory", 1); 27 | control_command_input_pub_ = nh_.advertise( 28 | "autopilot/control_command_input", 1); 29 | 30 | start_pub_ = nh_.advertise("autopilot/start", 1); 31 | force_hover_pub_ = nh_.advertise("autopilot/force_hover", 1); 32 | land_pub_ = nh_.advertise("autopilot/land", 1); 33 | off_pub_ = nh_.advertise("autopilot/off", 1); 34 | 35 | autopilot_feedback_sub_ = 36 | nh_.subscribe("autopilot/feedback", 10, 37 | &AutoPilotHelper::autopilotFeedbackCallback, this); 38 | } 39 | 40 | AutoPilotHelper::~AutoPilotHelper() {} 41 | 42 | bool AutoPilotHelper::feedbackAvailable() const { 43 | if (!first_autopilot_feedback_received_) { 44 | return false; 45 | } 46 | 47 | if (feedbackMessageAge() > kFeedbackValidTimeout_) { 48 | return false; 49 | } 50 | 51 | return true; 52 | } 53 | 54 | double AutoPilotHelper::feedbackMessageAge() const { 55 | if (!first_autopilot_feedback_received_) { 56 | // Just return a "very" large number 57 | return 100.0 * kFeedbackValidTimeout_; 58 | } 59 | 60 | return (ros::Time::now() - time_last_feedback_received_).toSec(); 61 | } 62 | 63 | bool AutoPilotHelper::stateEstimateAvailable() const { 64 | if (getCurrentStateEstimate().coordinate_frame == 65 | quadrotor_common::QuadStateEstimate::CoordinateFrame::INVALID) { 66 | return false; 67 | } 68 | 69 | return true; 70 | } 71 | 72 | bool AutoPilotHelper::waitForAutopilotFeedback( 73 | const double timeout_s, const double loop_rate_hz) const { 74 | const ros::Duration timeout(timeout_s); 75 | ros::Rate loop_rate(loop_rate_hz); 76 | const ros::Time start_wait = ros::Time::now(); 77 | while (ros::ok() && (ros::Time::now() - start_wait) <= timeout) { 78 | ros::spinOnce(); 79 | if (feedbackAvailable()) { 80 | return true; 81 | } 82 | loop_rate.sleep(); 83 | } 84 | 85 | return false; 86 | } 87 | 88 | bool AutoPilotHelper::waitForSpecificAutopilotState( 89 | const autopilot::States& state, const double timeout_s, 90 | const double loop_rate_hz) const { 91 | const ros::Duration timeout(timeout_s); 92 | ros::Rate loop_rate(loop_rate_hz); 93 | const ros::Time start_wait = ros::Time::now(); 94 | while (ros::ok() && (ros::Time::now() - start_wait) <= timeout) { 95 | ros::spinOnce(); 96 | if (feedbackAvailable() && getCurrentAutopilotState() == state) { 97 | return true; 98 | } 99 | loop_rate.sleep(); 100 | } 101 | 102 | return false; 103 | } 104 | 105 | autopilot::States AutoPilotHelper::getCurrentAutopilotState() const { 106 | switch (autopilot_feedback_.autopilot_state) { 107 | case autopilot_feedback_.OFF: 108 | return autopilot::States::OFF; 109 | case autopilot_feedback_.START: 110 | return autopilot::States::START; 111 | case autopilot_feedback_.HOVER: 112 | return autopilot::States::HOVER; 113 | case autopilot_feedback_.LAND: 114 | return autopilot::States::LAND; 115 | case autopilot_feedback_.EMERGENCY_LAND: 116 | return autopilot::States::EMERGENCY_LAND; 117 | case autopilot_feedback_.BREAKING: 118 | return autopilot::States::BREAKING; 119 | case autopilot_feedback_.GO_TO_POSE: 120 | return autopilot::States::GO_TO_POSE; 121 | case autopilot_feedback_.VELOCITY_CONTROL: 122 | return autopilot::States::VELOCITY_CONTROL; 123 | case autopilot_feedback_.REFERENCE_CONTROL: 124 | return autopilot::States::REFERENCE_CONTROL; 125 | case autopilot_feedback_.TRAJECTORY_CONTROL: 126 | return autopilot::States::TRAJECTORY_CONTROL; 127 | case autopilot_feedback_.COMMAND_FEEDTHROUGH: 128 | return autopilot::States::COMMAND_FEEDTHROUGH; 129 | case autopilot_feedback_.RC_MANUAL: 130 | return autopilot::States::RC_MANUAL; 131 | default: 132 | return autopilot::States::OFF; 133 | } 134 | } 135 | 136 | ros::Duration AutoPilotHelper::getCurrentControlCommandDelay() const { 137 | return autopilot_feedback_.control_command_delay; 138 | } 139 | 140 | ros::Duration AutoPilotHelper::getCurrentControlComputationTime() const { 141 | return autopilot_feedback_.control_computation_time; 142 | } 143 | 144 | ros::Duration AutoPilotHelper::getCurrentTrajectoryExecutionLeftDuration() 145 | const { 146 | return autopilot_feedback_.trajectory_execution_left_duration; 147 | } 148 | 149 | int AutoPilotHelper::getCurrentTrajectoriesLeftInQueue() const { 150 | return autopilot_feedback_.trajectories_left_in_queue; 151 | } 152 | 153 | quadrotor_common::TrajectoryPoint AutoPilotHelper::getCurrentReferenceState() 154 | const { 155 | return quadrotor_common::TrajectoryPoint(autopilot_feedback_.reference_state); 156 | } 157 | 158 | Eigen::Vector3d AutoPilotHelper::getCurrentReferencePosition() const { 159 | return quadrotor_common::geometryToEigen( 160 | autopilot_feedback_.reference_state.pose.position); 161 | } 162 | 163 | Eigen::Vector3d AutoPilotHelper::getCurrentReferenceVelocity() const { 164 | return quadrotor_common::geometryToEigen( 165 | autopilot_feedback_.reference_state.velocity.linear); 166 | } 167 | 168 | Eigen::Quaterniond AutoPilotHelper::getCurrentReferenceOrientation() const { 169 | return quadrotor_common::geometryToEigen( 170 | autopilot_feedback_.reference_state.pose.orientation); 171 | } 172 | 173 | double AutoPilotHelper::getCurrentReferenceHeading() const { 174 | return quadrotor_common::quaternionToEulerAnglesZYX( 175 | quadrotor_common::geometryToEigen( 176 | autopilot_feedback_.reference_state.pose.orientation)) 177 | .z(); 178 | } 179 | 180 | quadrotor_common::QuadStateEstimate AutoPilotHelper::getCurrentStateEstimate() 181 | const { 182 | return quadrotor_common::QuadStateEstimate( 183 | autopilot_feedback_.state_estimate); 184 | } 185 | 186 | Eigen::Vector3d AutoPilotHelper::getCurrentPositionEstimate() const { 187 | return quadrotor_common::geometryToEigen( 188 | autopilot_feedback_.state_estimate.pose.pose.position); 189 | } 190 | 191 | Eigen::Vector3d AutoPilotHelper::getCurrentVelocityEstimate() const { 192 | return quadrotor_common::geometryToEigen( 193 | autopilot_feedback_.state_estimate.twist.twist.linear); 194 | } 195 | 196 | Eigen::Quaterniond AutoPilotHelper::getCurrentOrientationEstimate() const { 197 | return quadrotor_common::geometryToEigen( 198 | autopilot_feedback_.state_estimate.pose.pose.orientation); 199 | } 200 | 201 | double AutoPilotHelper::getCurrentHeadingEstimate() const { 202 | return quadrotor_common::quaternionToEulerAnglesZYX( 203 | quadrotor_common::geometryToEigen( 204 | autopilot_feedback_.state_estimate.pose.pose.orientation)) 205 | .z(); 206 | } 207 | 208 | Eigen::Vector3d AutoPilotHelper::getCurrentPositionError() const { 209 | return getCurrentPositionEstimate() - getCurrentReferencePosition(); 210 | } 211 | 212 | Eigen::Vector3d AutoPilotHelper::getCurrentVelocityError() const { 213 | return getCurrentVelocityEstimate() - getCurrentReferenceVelocity(); 214 | } 215 | 216 | Eigen::Quaterniond AutoPilotHelper::getCurrentOrientationError() const { 217 | return getCurrentReferenceOrientation().inverse() * 218 | getCurrentOrientationEstimate(); 219 | } 220 | 221 | double AutoPilotHelper::getCurrentHeadingError() const { 222 | return quadrotor_common::wrapMinusPiToPi(getCurrentHeadingEstimate() - 223 | getCurrentReferenceHeading()); 224 | } 225 | 226 | void AutoPilotHelper::sendPoseCommand(const Eigen::Vector3d& position, 227 | const double heading) const { 228 | geometry_msgs::PoseStamped pose_cmd; 229 | pose_cmd.pose.position.x = position.x(); 230 | pose_cmd.pose.position.y = position.y(); 231 | pose_cmd.pose.position.z = position.z(); 232 | pose_cmd.pose.orientation = 233 | quadrotor_common::eigenToGeometry(Eigen::Quaterniond( 234 | Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(heading), 235 | Eigen::Vector3d::UnitZ()))); 236 | 237 | pose_pub_.publish(pose_cmd); 238 | } 239 | 240 | void AutoPilotHelper::sendVelocityCommand(const Eigen::Vector3d& velocity, 241 | const double heading_rate) const { 242 | geometry_msgs::TwistStamped vel_cmd; 243 | vel_cmd.twist.linear.x = velocity.x(); 244 | vel_cmd.twist.linear.y = velocity.y(); 245 | vel_cmd.twist.linear.z = velocity.z(); 246 | vel_cmd.twist.angular.z = heading_rate; 247 | 248 | velocity_pub_.publish(vel_cmd); 249 | } 250 | 251 | void AutoPilotHelper::sendReferenceState( 252 | const quadrotor_common::TrajectoryPoint& trajectory_point) const { 253 | reference_state_pub_.publish(trajectory_point.toRosMessage()); 254 | } 255 | 256 | void AutoPilotHelper::sendTrajectory( 257 | const quadrotor_common::Trajectory& trajectory) const { 258 | trajectory_pub_.publish(trajectory.toRosMessage()); 259 | } 260 | 261 | void AutoPilotHelper::sendControlCommandInput( 262 | const quadrotor_common::ControlCommand& control_command) const { 263 | control_command_input_pub_.publish(control_command.toRosMessage()); 264 | } 265 | 266 | void AutoPilotHelper::sendStart() const { 267 | start_pub_.publish(std_msgs::Empty()); 268 | } 269 | 270 | void AutoPilotHelper::sendForceHover() const { 271 | force_hover_pub_.publish(std_msgs::Empty()); 272 | } 273 | 274 | void AutoPilotHelper::sendLand() const { land_pub_.publish(std_msgs::Empty()); } 275 | 276 | void AutoPilotHelper::sendOff() const { off_pub_.publish(std_msgs::Empty()); } 277 | 278 | void AutoPilotHelper::autopilotFeedbackCallback( 279 | const quadrotor_msgs::AutopilotFeedback::ConstPtr& msg) { 280 | time_last_feedback_received_ = ros::Time::now(); 281 | 282 | autopilot_feedback_ = *msg; 283 | 284 | if (!first_autopilot_feedback_received_) { 285 | first_autopilot_feedback_received_ = true; 286 | } 287 | } 288 | 289 | } // namespace autopilot_helper 290 | -------------------------------------------------------------------------------- /utils/manual_flight_assistant/src/manual_flight_assistant.cpp: -------------------------------------------------------------------------------- 1 | #include "manual_flight_assistant/manual_flight_assistant.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "manual_flight_assistant/joypad_axes_buttons.h" 11 | #include "manual_flight_assistant/rc_channels_switches.h" 12 | 13 | namespace manual_flight_assistant { 14 | 15 | ManualFlightAssistant::ManualFlightAssistant(const ros::NodeHandle& nh, 16 | const ros::NodeHandle& pnh) 17 | : nh_(nh), 18 | pnh_(pnh), 19 | time_last_joypad_msg_(), 20 | sbus_command_(), 21 | previous_sbus_command_(), 22 | time_last_sbus_msg_(), 23 | sbus_needs_to_go_through_zero_(true), 24 | velocity_command_is_zero_(true) { 25 | loadParameters(); 26 | 27 | joypad_command_ = sensor_msgs::Joy(); 28 | joypad_command_.axes = std::vector(8, 0); 29 | joypad_command_.buttons = std::vector(8, 0); 30 | previous_joypad_command_ = joypad_command_; 31 | 32 | manual_desired_velocity_pub_ = nh_.advertise( 33 | "autopilot/velocity_command", 1); 34 | start_pub_ = nh_.advertise("autopilot/start", 1); 35 | land_pub_ = nh_.advertise("autopilot/land", 1); 36 | 37 | joypad_sub_ = 38 | nh_.subscribe("joy", 1, &ManualFlightAssistant::joyCallback, this); 39 | rc_sbus_sub_ = nh_.subscribe("received_sbus_message", 1, 40 | &ManualFlightAssistant::rcSbusCallback, this); 41 | 42 | main_loop_timer_ = nh_.createTimer(ros::Duration(1.0 / kLoopFrequency_), 43 | &ManualFlightAssistant::mainLoop, this); 44 | } 45 | 46 | ManualFlightAssistant::~ManualFlightAssistant() {} 47 | 48 | void ManualFlightAssistant::mainLoop(const ros::TimerEvent& time) { 49 | if (rcSbusAvailable()) { 50 | geometry_msgs::TwistStamped velocity_command; 51 | velocity_command.header.stamp = ros::Time::now(); 52 | 53 | // Coefficients to linearly map sbus values to an interval of [-1, 1] 54 | const double value_map_a = 55 | 2.0 / (sbus_bridge::SBusMsg::kMaxCmd - sbus_bridge::SBusMsg::kMinCmd); 56 | const double value_map_b = 57 | 1.0 - (value_map_a * sbus_bridge::SBusMsg::kMaxCmd); 58 | 59 | if (abs(sbus_command_.channels[sbus_bridge::channel_mapping::kPitch] - 60 | sbus_bridge::SBusMsg::kMeanCmd) > sbus_axes_zero_tolerance_) { 61 | const double normalized_command = std::max( 62 | std::min(value_map_a * 63 | sbus_command_ 64 | .channels[sbus_bridge::channel_mapping::kPitch] + 65 | value_map_b, 66 | 1.0), 67 | -1.0); 68 | velocity_command.twist.linear.x = vmax_xy_ * normalized_command; 69 | } 70 | if (abs(sbus_command_.channels[sbus_bridge::channel_mapping::kRoll] - 71 | sbus_bridge::SBusMsg::kMeanCmd) > sbus_axes_zero_tolerance_) { 72 | const double normalized_command = std::max( 73 | std::min( 74 | value_map_a * sbus_command_ 75 | .channels[sbus_bridge::channel_mapping::kRoll] + 76 | value_map_b, 77 | 1.0), 78 | -1.0); 79 | velocity_command.twist.linear.y = -vmax_xy_ * normalized_command; 80 | } 81 | if (abs(sbus_command_.channels[sbus_bridge::channel_mapping::kThrottle] - 82 | sbus_bridge::SBusMsg::kMeanCmd) > 3 * sbus_axes_zero_tolerance_) { 83 | const double normalized_command = std::max( 84 | std::min( 85 | value_map_a * 86 | sbus_command_ 87 | .channels[sbus_bridge::channel_mapping::kThrottle] + 88 | value_map_b, 89 | 1.0), 90 | -1.0); 91 | velocity_command.twist.linear.z = vmax_z_ * normalized_command; 92 | } 93 | if (abs(sbus_command_.channels[sbus_bridge::channel_mapping::kYaw] - 94 | sbus_bridge::SBusMsg::kMeanCmd) > sbus_axes_zero_tolerance_) { 95 | const double normalized_command = std::max( 96 | std::min( 97 | value_map_a * sbus_command_ 98 | .channels[sbus_bridge::channel_mapping::kYaw] + 99 | value_map_b, 100 | 1.0), 101 | -1.0); 102 | velocity_command.twist.angular.z = -rmax_yaw_ * normalized_command; 103 | } 104 | 105 | publishVelocityCommand(velocity_command); 106 | } else if (joypadAvailable()) { 107 | geometry_msgs::TwistStamped velocity_command; 108 | velocity_command.header.stamp = ros::Time::now(); 109 | 110 | if (fabs(joypad_command_.axes[joypad::axes::kX]) > 111 | joypad_axes_zero_tolerance_) { 112 | velocity_command.twist.linear.x = 113 | vmax_xy_ * joypad_command_.axes[joypad::axes::kX]; 114 | } 115 | if (fabs(joypad_command_.axes[joypad::axes::kY]) > 116 | joypad_axes_zero_tolerance_) { 117 | velocity_command.twist.linear.y = 118 | vmax_xy_ * joypad_command_.axes[joypad::axes::kY]; 119 | } 120 | if (fabs(joypad_command_.axes[joypad::axes::kZ]) > 121 | joypad_axes_zero_tolerance_) { 122 | velocity_command.twist.linear.z = 123 | vmax_z_ * joypad_command_.axes[joypad::axes::kZ]; 124 | } 125 | if (fabs(joypad_command_.axes[joypad::axes::kYaw]) > 126 | joypad_axes_zero_tolerance_) { 127 | velocity_command.twist.angular.z = 128 | rmax_yaw_ * joypad_command_.axes[joypad::axes::kYaw]; 129 | } 130 | 131 | publishVelocityCommand(velocity_command); 132 | 133 | // Start and Land Buttons 134 | if (!previous_joypad_command_.buttons.empty()) { 135 | if (joypad_command_.buttons[joypad::buttons::kGreen] && 136 | !previous_joypad_command_.buttons[joypad::buttons::kGreen]) { 137 | start_pub_.publish(std_msgs::Empty()); 138 | } 139 | if (joypad_command_.buttons[joypad::buttons::kBlue] && 140 | !previous_joypad_command_.buttons[joypad::buttons::kBlue]) { 141 | land_pub_.publish(std_msgs::Empty()); 142 | } 143 | } 144 | } else { 145 | // Publish zero velocity command 146 | geometry_msgs::TwistStamped velocity_command; 147 | velocity_command.header.stamp = ros::Time::now(); 148 | publishVelocityCommand(velocity_command); 149 | } 150 | 151 | // Check whether sbus passes zero if it needs to 152 | if (sbus_needs_to_go_through_zero_ && 153 | (ros::Time::now() - time_last_sbus_msg_) <= 154 | ros::Duration(sbus_timeout_) && 155 | abs(sbus_command_.channels[sbus_bridge::channel_mapping::kPitch] - 156 | sbus_bridge::SBusMsg::kMeanCmd) < sbus_axes_zero_tolerance_ && 157 | abs(sbus_command_.channels[sbus_bridge::channel_mapping::kRoll] - 158 | sbus_bridge::SBusMsg::kMeanCmd) < sbus_axes_zero_tolerance_ && 159 | abs(sbus_command_.channels[sbus_bridge::channel_mapping::kThrottle] - 160 | sbus_bridge::SBusMsg::kMeanCmd) < 3 * sbus_axes_zero_tolerance_ && 161 | abs(sbus_command_.channels[sbus_bridge::channel_mapping::kYaw] - 162 | sbus_bridge::SBusMsg::kMeanCmd) < sbus_axes_zero_tolerance_) { 163 | sbus_needs_to_go_through_zero_ = false; 164 | } 165 | } 166 | 167 | void ManualFlightAssistant::joyCallback(const sensor_msgs::Joy::ConstPtr& msg) { 168 | previous_joypad_command_ = joypad_command_; 169 | joypad_command_ = *msg; 170 | time_last_joypad_msg_ = ros::Time::now(); 171 | } 172 | 173 | void ManualFlightAssistant::rcSbusCallback( 174 | const sbus_bridge::SbusRosMessage::ConstPtr& msg) { 175 | previous_sbus_command_ = sbus_command_; 176 | sbus_command_ = *msg; 177 | if (!sbus_command_.frame_lost && !sbus_command_.failsafe) { 178 | // This also accounts for the fact that the sbus message should not be 179 | // invalid for too long 180 | time_last_sbus_msg_ = ros::Time::now(); 181 | } 182 | } 183 | 184 | bool ManualFlightAssistant::joypadAvailable() { 185 | if ((ros::Time::now() - time_last_joypad_msg_) > 186 | ros::Duration(joypad_timeout_)) { 187 | return false; 188 | } 189 | 190 | return true; 191 | } 192 | 193 | bool ManualFlightAssistant::rcSbusAvailable() { 194 | bool available = true; 195 | 196 | if (sbus_command_.channels[sbus_bridge::channel_mapping::kArming] > 197 | sbus_bridge::SBusMsg::kMeanCmd) { 198 | // Pure manual flight 199 | available = false; 200 | } else if (sbus_command_ 201 | .channels[sbus_bridge::channel_mapping::kGamepadMode] < 202 | (sbus_bridge::SBusMsg::kMeanCmd + sbus_axes_zero_tolerance_)) { 203 | // RC is not in gamepad flight mode 204 | available = false; 205 | } else if (sbus_needs_to_go_through_zero_) { 206 | available = false; 207 | } else if ((ros::Time::now() - time_last_sbus_msg_) > 208 | ros::Duration(sbus_timeout_)) { 209 | available = false; 210 | } 211 | 212 | if (!available) { 213 | sbus_needs_to_go_through_zero_ = true; 214 | } 215 | 216 | return available; 217 | } 218 | 219 | void ManualFlightAssistant::publishVelocityCommand( 220 | const geometry_msgs::TwistStamped& velocity_command) { 221 | if (quadrotor_common::geometryToEigen(velocity_command.twist.linear).norm() <= 222 | kVelocityCommandZeroThreshold_ && 223 | fabs(velocity_command.twist.angular.z) <= 224 | kVelocityCommandZeroThreshold_) { 225 | if (!velocity_command_is_zero_) { 226 | velocity_command_is_zero_ = true; 227 | // Publish one zero velocity command afterwards stop publishing 228 | const geometry_msgs::TwistStamped zero_command; 229 | manual_desired_velocity_pub_.publish(zero_command); 230 | } 231 | } else if (velocity_command_is_zero_) { 232 | velocity_command_is_zero_ = false; 233 | } 234 | 235 | if (!velocity_command_is_zero_) { 236 | manual_desired_velocity_pub_.publish(velocity_command); 237 | } 238 | } 239 | 240 | bool ManualFlightAssistant::loadParameters() { 241 | if (!quadrotor_common::getParam("joypad_timeout", joypad_timeout_, pnh_)) { 242 | return false; 243 | } 244 | if (!quadrotor_common::getParam("sbus_timeout", sbus_timeout_, pnh_)) { 245 | return false; 246 | } 247 | if (!quadrotor_common::getParam("joypad_axes_zero_tolerance", 248 | joypad_axes_zero_tolerance_, pnh_)) { 249 | return false; 250 | } 251 | if (!quadrotor_common::getParam("sbus_axes_zero_tolerance", 252 | sbus_axes_zero_tolerance_, pnh_)) { 253 | return false; 254 | } 255 | if (!quadrotor_common::getParam("vmax_xy", vmax_xy_, pnh_)) { 256 | return false; 257 | } 258 | if (!quadrotor_common::getParam("vmax_z", vmax_z_, pnh_)) { 259 | return false; 260 | } 261 | if (!quadrotor_common::getParam("rmax_yaw", rmax_yaw_, pnh_)) { 262 | return false; 263 | } 264 | 265 | return true; 266 | } 267 | 268 | } // namespace manual_flight_assistant 269 | 270 | int main(int argc, char** argv) { 271 | ros::init(argc, argv, "manual_flight_assistant"); 272 | manual_flight_assistant::ManualFlightAssistant manual_flight_assistant; 273 | 274 | ros::spin(); 275 | return 0; 276 | } 277 | -------------------------------------------------------------------------------- /trajectory_planning/polynomial_trajectories/src/constrained_polynomial_trajectories.cpp: -------------------------------------------------------------------------------- 1 | #include "polynomial_trajectories/constrained_polynomial_trajectories.h" 2 | 3 | #include 4 | 5 | #include "polynomial_trajectories/polynomial_trajectories_common.h" 6 | 7 | namespace polynomial_trajectories { 8 | namespace constrained_polynomial_trajectories { 9 | 10 | PolynomialTrajectory computeTimeOptimalTrajectory( 11 | const quadrotor_common::TrajectoryPoint& s0, 12 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 13 | const double max_velocity, const double max_normalized_thrust, 14 | const double max_roll_pitch_rate) { 15 | if (!isStartAndEndStateFeasibleUnderConstraints( 16 | s0, s1, max_velocity, max_normalized_thrust, max_roll_pitch_rate)) { 17 | ROS_ERROR( 18 | "[%s] Desired start or end state of trajectory is not feasible " 19 | "under given velocity and thrust constraints.", 20 | ros::this_node::getName().c_str()); 21 | return PolynomialTrajectory(); 22 | } 23 | 24 | // Compute initial trajectory duration from a trapezoidal trajectory 25 | // considering max velocity and max thrust 26 | double distance = (s1.position - s0.position).norm(); 27 | double n_z = ((s1.position - s0.position).normalized()).z(); 28 | double acc_towards_s1_max = std::max( 29 | -n_z * 9.81 + sqrt(9.81 * 9.81 * (n_z * n_z - 1.0) + 30 | max_normalized_thrust * max_normalized_thrust), 31 | -n_z * 9.81 - sqrt(9.81 * 9.81 * (n_z * n_z - 1.0) + 32 | max_normalized_thrust * max_normalized_thrust)); 33 | 34 | // value according to old implementation, will be overwritten 35 | double init_trajectory_duration = distance / max_velocity; 36 | if (max_velocity >= sqrt(acc_towards_s1_max * distance)) { 37 | init_trajectory_duration = 2 * sqrt(distance / acc_towards_s1_max); 38 | } else { 39 | init_trajectory_duration = 40 | max_velocity / acc_towards_s1_max + distance / max_velocity; 41 | } 42 | 43 | // Compute initial trajectory such that limits must be violated 44 | PolynomialTrajectory initial_trajectory; 45 | initial_trajectory.trajectory_type = 46 | polynomial_trajectories::TrajectoryType::FULLY_CONSTRAINED; 47 | initial_trajectory.start_state = s0; 48 | initial_trajectory.start_state.time_from_start = ros::Duration(0.0); 49 | initial_trajectory.end_state = s1; 50 | initial_trajectory.number_of_segments = 1; 51 | initial_trajectory.T = ros::Duration(init_trajectory_duration); 52 | initial_trajectory.end_state.time_from_start = initial_trajectory.T; 53 | initial_trajectory.coeff = implementation::computeTrajectoryCoeff( 54 | s0, s1, order_of_continuity, initial_trajectory.T.toSec()); 55 | initial_trajectory.segment_times.resize(1); 56 | initial_trajectory.segment_times(0) = initial_trajectory.T.toSec(); 57 | 58 | PolynomialTrajectory trajectory = initial_trajectory; 59 | Eigen::Vector3d desired_maxima = 60 | Eigen::Vector3d(max_velocity, max_normalized_thrust, max_roll_pitch_rate); 61 | Eigen::Vector3d prev_maxima; 62 | computeQuadRelevantMaxima(trajectory, &prev_maxima.x(), &prev_maxima.y(), 63 | &prev_maxima.z()); 64 | 65 | while (prev_maxima.x() <= 1.01 * desired_maxima.x() && 66 | prev_maxima.y() <= 1.01 * desired_maxima.y() && 67 | prev_maxima.z() <= 1.01 * desired_maxima.z()) { 68 | if ((prev_maxima.x() >= 0.99 * desired_maxima.x() || 69 | prev_maxima.y() >= 0.99 * desired_maxima.y() || 70 | prev_maxima.z() >= 0.99 * desired_maxima.z()) && 71 | (prev_maxima.x() <= desired_maxima.x() && 72 | prev_maxima.y() <= desired_maxima.y() && 73 | prev_maxima.z() <= desired_maxima.z())) { 74 | return trajectory; 75 | } 76 | 77 | trajectory.T = ros::Duration(0.9 * trajectory.T.toSec()); 78 | trajectory.end_state.time_from_start = trajectory.T; 79 | trajectory.segment_times(0) = trajectory.T.toSec(); 80 | trajectory.coeff = implementation::computeTrajectoryCoeff( 81 | s0, s1, order_of_continuity, trajectory.T.toSec()); 82 | 83 | computeQuadRelevantMaxima(trajectory, &prev_maxima.x(), &prev_maxima.y(), 84 | &prev_maxima.z()); 85 | } 86 | 87 | // compute gradient of maxima with respect to T 88 | Eigen::Vector3d maxima_gradient = implementation::computeMaximaGradient( 89 | trajectory, prev_maxima, order_of_continuity); 90 | 91 | // Iteratively optimize execution time 92 | const int max_iterations = 15; 93 | for (int i = 0; i < max_iterations; i++) { 94 | Eigen::Vector3d new_intersections = 95 | (desired_maxima - prev_maxima).cwiseQuotient(maxima_gradient) + 96 | trajectory.T.toSec() * Eigen::Vector3d::Ones(); 97 | 98 | for (int i = 0; i < 3; i++) { 99 | if (fabs(maxima_gradient(i)) < 5e-2) { 100 | // make it negative since we will filter this out later 101 | new_intersections(i) = -1.0; 102 | } 103 | } 104 | 105 | Eigen::Vector3d prev_gradient = maxima_gradient; 106 | PolynomialTrajectory prev_trajectory = trajectory; 107 | bool exhaustivly_search_minimum = false; 108 | Eigen::Vector3d maxima; 109 | 110 | if (new_intersections.maxCoeff() > initial_trajectory.T.toSec()) { 111 | // recompute trajectory with new duration 112 | trajectory.T = ros::Duration(new_intersections.maxCoeff()); 113 | trajectory.end_state.time_from_start = trajectory.T; 114 | trajectory.segment_times(0) = trajectory.T.toSec(); 115 | trajectory.coeff = implementation::computeTrajectoryCoeff( 116 | s0, s1, order_of_continuity, trajectory.T.toSec()); 117 | 118 | computeQuadRelevantMaxima(trajectory, &maxima.x(), &maxima.y(), 119 | &maxima.z()); 120 | 121 | if (maxima.x() <= 1.01 * desired_maxima.x() && 122 | maxima.y() <= 1.01 * desired_maxima.y() && 123 | maxima.z() <= 1.01 * desired_maxima.z()) { 124 | // we are close enough at the limits so we stop here 125 | break; 126 | } 127 | 128 | // compute gradient of maxima with respect to T 129 | maxima_gradient = implementation::computeMaximaGradient( 130 | trajectory, maxima, order_of_continuity); 131 | 132 | // check if gradient flipped sign 133 | if ((prev_gradient.cwiseProduct(maxima_gradient)).minCoeff() < 0.0) { 134 | trajectory = prev_trajectory; 135 | maxima_gradient = prev_gradient; 136 | exhaustivly_search_minimum = true; 137 | } 138 | } else { 139 | exhaustivly_search_minimum = true; 140 | } 141 | 142 | if (exhaustivly_search_minimum) { 143 | for (int j = i; j < max_iterations; j++) { 144 | trajectory.T = ros::Duration(1.1 * trajectory.T.toSec()); 145 | trajectory.end_state.time_from_start = trajectory.T; 146 | trajectory.segment_times(0) = trajectory.T.toSec(); 147 | trajectory.coeff = implementation::computeTrajectoryCoeff( 148 | s0, s1, order_of_continuity, trajectory.T.toSec()); 149 | 150 | computeQuadRelevantMaxima(trajectory, &maxima.x(), &maxima.y(), 151 | &maxima.z()); 152 | if ((maxima.x() > desired_maxima.x() && 153 | maxima.x() > prev_maxima.x() + 1e-6) || 154 | (maxima.y() > desired_maxima.y() && 155 | maxima.y() > prev_maxima.y() + 1e-6) || 156 | (maxima.z() > desired_maxima.z() && 157 | maxima.z() > prev_maxima.z() + 1e-6)) { 158 | trajectory = prev_trajectory; 159 | break; 160 | } 161 | if (maxima.x() <= 1.01 * desired_maxima.x() && 162 | maxima.y() <= 1.01 * desired_maxima.y() && 163 | maxima.z() <= 1.01 * desired_maxima.z()) { 164 | // Reached desired maxima so we take this trajectory 165 | break; 166 | } 167 | prev_maxima = maxima; 168 | prev_trajectory = trajectory; 169 | } 170 | 171 | break; 172 | } 173 | prev_maxima = maxima; 174 | } 175 | 176 | return trajectory; 177 | } 178 | 179 | PolynomialTrajectory computeFixedTimeTrajectory( 180 | const quadrotor_common::TrajectoryPoint& s0, 181 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 182 | const double execution_time) { 183 | PolynomialTrajectory trajectory; 184 | trajectory.trajectory_type = 185 | polynomial_trajectories::TrajectoryType::FULLY_CONSTRAINED; 186 | trajectory.start_state = s0; 187 | trajectory.start_state.time_from_start = ros::Duration(0.0); 188 | trajectory.end_state = s1; 189 | trajectory.number_of_segments = 1; 190 | trajectory.T = ros::Duration(execution_time); 191 | trajectory.end_state.time_from_start = trajectory.T; 192 | trajectory.coeff = implementation::computeTrajectoryCoeff( 193 | s0, s1, order_of_continuity, execution_time); 194 | 195 | trajectory.segment_times.resize(1); 196 | trajectory.segment_times(0) = trajectory.T.toSec(); 197 | 198 | return trajectory; 199 | } 200 | 201 | Eigen::MatrixXd implementation::computeConstraintMatriceA( 202 | const int order_of_continuity, const double t) { 203 | int number_of_coefficients = 2 * order_of_continuity; 204 | 205 | Eigen::MatrixXd A = 206 | Eigen::MatrixXd::Zero(order_of_continuity, number_of_coefficients); 207 | 208 | for (int i = 0; i < order_of_continuity; i++) { 209 | A.row(i) = (dVec(number_of_coefficients, i).asDiagonal() * 210 | tVec(number_of_coefficients, i, t)) 211 | .transpose(); 212 | } 213 | return A; 214 | } 215 | 216 | Eigen::VectorXd implementation::computeConstraintMatriceB( 217 | const int order_of_continuity, 218 | const quadrotor_common::TrajectoryPoint& state, const int axis) { 219 | Eigen::VectorXd b = Eigen::VectorXd::Zero(order_of_continuity); 220 | for (int i = 0; i < order_of_continuity; i++) { 221 | if (i == 0) b[i] = state.position(axis); 222 | if (i == 1) b[i] = state.velocity(axis); 223 | if (i == 2) b[i] = state.acceleration(axis); 224 | if (i == 3) b[i] = state.jerk(axis); 225 | } 226 | return b; 227 | } 228 | 229 | std::vector implementation::computeTrajectoryCoeff( 230 | const quadrotor_common::TrajectoryPoint& s0, 231 | const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity, 232 | const double T) { 233 | int number_of_coefficients = 2 * order_of_continuity; 234 | Eigen::MatrixXd p_coeff = Eigen::MatrixXd::Zero(3, number_of_coefficients); 235 | 236 | // solve the problem for each axis 237 | for (int axis = 0; axis < 3; axis++) { 238 | Eigen::MatrixXd A = 239 | Eigen::MatrixXd::Zero(number_of_coefficients, number_of_coefficients); 240 | Eigen::VectorXd b = Eigen::VectorXd::Zero(number_of_coefficients); 241 | 242 | A.topLeftCorner(order_of_continuity, number_of_coefficients) = 243 | implementation::computeConstraintMatriceA(order_of_continuity, 0.0); 244 | A.bottomLeftCorner(order_of_continuity, number_of_coefficients) = 245 | implementation::computeConstraintMatriceA(order_of_continuity, T); 246 | 247 | b.head(order_of_continuity) = implementation::computeConstraintMatriceB( 248 | order_of_continuity, s0, axis); 249 | b.tail(order_of_continuity) = implementation::computeConstraintMatriceB( 250 | order_of_continuity, s1, axis); 251 | 252 | p_coeff.row(axis) = A.inverse() * b; 253 | } 254 | 255 | std::vector coeff_vec; 256 | coeff_vec.push_back(p_coeff); 257 | 258 | return coeff_vec; 259 | } 260 | 261 | Eigen::Vector3d implementation::computeMaximaGradient( 262 | const PolynomialTrajectory& trajectory, const Eigen::Vector3d& maxima, 263 | const double order_of_continuity) { 264 | PolynomialTrajectory gradient_trajectory = trajectory; 265 | gradient_trajectory.T = ros::Duration(1.01 * trajectory.T.toSec()); 266 | gradient_trajectory.segment_times(0) = gradient_trajectory.T.toSec(); 267 | gradient_trajectory.coeff = implementation::computeTrajectoryCoeff( 268 | trajectory.start_state, trajectory.end_state, order_of_continuity, 269 | gradient_trajectory.T.toSec()); 270 | Eigen::Vector3d gradient_trajectory_maxima; 271 | computeQuadRelevantMaxima( 272 | gradient_trajectory, &gradient_trajectory_maxima.x(), 273 | &gradient_trajectory_maxima.y(), &gradient_trajectory_maxima.z()); 274 | 275 | Eigen::Vector3d gradient = (gradient_trajectory_maxima - maxima) / 276 | (gradient_trajectory.T - trajectory.T).toSec(); 277 | 278 | return gradient; 279 | } 280 | 281 | } // namespace constrained_polynomial_trajectories 282 | 283 | } // namespace polynomial_trajectories 284 | -------------------------------------------------------------------------------- /documents/theory_and_math/library.bib: -------------------------------------------------------------------------------- 1 | % JOURNALS 2 | % - robotics 3 | @STRING{ijrr = "Int. J. Robot. Research" } 4 | @STRING{tro = "{IEEE} Trans. Robot." } 5 | @STRING{tra = "{IEEE} Trans. Robot. Autom." } 6 | @STRING{ral = "{IEEE} Robot. Autom. Lett." } 7 | % before TRO 8 | @STRING{ras = "J. Robot. and Auton. Syst." } 9 | @STRING{jfr = "J. Field Robot." } 10 | @STRING{fsr = "Field and Service Robot." } 11 | @STRING{ar = "Auton. Robots" } 12 | @STRING{ijhr = "Int. J. Humanoid Robot." } 13 | @STRING{mech = "J. Mechatronics" } 14 | @STRING{jirs = "J. Intell. Robot. Syst." } 15 | @STRING{ijars = "Int. J. Adv. Robot. Syst." } 16 | @STRING{jgcd = "J. Guidance, Control, and Dynamics" } 17 | % - computer vision 18 | @STRING{ijcv = "Int. J. Comput. Vis." } 19 | @STRING{cviu = "Comput. Vis. Image. Und." } 20 | @STRING{jmiv = "J. Math. Imaging Vis." } 21 | @STRING{pami = "{IEEE} Trans. Pattern Anal. Machine Intell." } 22 | @STRING{jivc = "Image Vis. Comput." } 23 | @STRING{jrtip = "J. Real-Time Image Process." } 24 | @STRING{tip = "{IEEE} Trans. Image Process." } 25 | % - neuroscience 26 | @STRING{fns = "Front. Neurosci." } 27 | @STRING{tnnls = "{IEEE} Trans. Neural Netw. Learn. Syst." } 28 | @STRING{tnn = "{IEEE} Trans. Neural Netw." } 29 | @STRING{nn = "Neural Netw." } 30 | @STRING{nnc = "{IEEE} Int. Conf. Neural Netw." } 31 | @STRING{tbcas = "{IEEE} Trans. Biomed. Circuits Syst." } 32 | @STRING{tcsi = "{IEEE} Trans. Circuits Syst. I, Reg. Papers" } 33 | % - misc 34 | @STRING{ac = "{IEEE} Trans. Autom. Control" } 35 | @STRING{aes = "{IEEE} Trans. Aerosp. Electron. Syst." } 36 | @STRING{sp = "{IEEE} Trans. Signal Process." } 37 | @STRING{it = "{IEEE} Trans. Inf. Theory" } 38 | @STRING{ieee = "Proc. {IEEE}" } 39 | @STRING{ssc = "{IEEE} J. Solid-State Circuits" } 40 | @STRING{ai = "J. Artificial Intell." } 41 | @STRING{jbe = "J. Basic Eng." } 42 | @STRING{acmcs = "{ACM} Comput. Surveys" } 43 | @STRING{tsmcb = "{IEEE} Trans. Syst. Man, Cybern. B, Cybern." } 44 | @STRING{tsmca = "{IEEE} Trans. Syst. Man, Cybern. A, Syst., Humans" } 45 | @STRING{vcip = "J. Visual Comm. Image Representation" } 46 | @STRING{nme = "Int. J. Numerical Methods in Eng." } 47 | % MAGAZINES 48 | @STRING{ram = "{IEEE} Robot. Autom. Mag." } 49 | @STRING{mcs = "{IEEE} Control Systems" } 50 | % CONFERENCES 51 | % - robotics 52 | @STRING{icra = "{IEEE} Int. Conf. Robot. Autom. (ICRA)" } 53 | @STRING{iros = "IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS)" } 54 | @STRING{isrr = "Proc. Int. Symp. Robot. Research (ISRR)" } 55 | @STRING{rss = "Robotics: Science and Systems (RSS)" } 56 | @STRING{icar = "{IEEE} Int. Conf. Adv. Robot. (ICAR)" } 57 | @STRING{emav = "Eur. Micro Aerial Vehicle Conf." } 58 | @STRING{ssrr = "{IEEE} Int. Symp. Safety, Security, and Rescue Robot. (SSRR)" } 59 | @STRING{ecmr = "Eur. Conf. Mobile Robots (ECMR)" } 60 | @STRING{clawar = "Int. Conf. Climbing and Walking Robots (CLAWAR)" } 61 | @STRING{aeroc = "{IEEE} Aerosp. Conf." } 62 | @STRING{iser = "Int. Symp. Experimental Robotics (ISER)" } 63 | @STRING{robio = "{IEEE} Int. Conf. Robot. Biomimetics (ROBIO)" } 64 | @STRING{icaim = "{IEEE/ASME} Int. Conf. Adv. Intell. Mechatronics" } 65 | @STRING{isr = "Int. Symp. Robotics (ISR)" } 66 | @STRING{icuas = "{IEEE} Int. Conf. Unmanned Aircraft Syst. (ICUAS)" } 67 | @STRING{ifac = "{IFAC} World Congress" } 68 | @STRING{case = "{IEEE} Int. Conf. Automation Science and Engineering (CASE)" } 69 | % - vision 70 | @STRING{cvpr = "{IEEE} Int. Conf. Comput. Vis. Pattern Recog. (CVPR)" } 71 | @STRING{cvprw = "{IEEE} Int. Conf. Comput. Vis. Pattern Recog. Workshops 72 | (CVPRW)" } 73 | @STRING{iccv = "Int. Conf. Comput. Vis. (ICCV)" } 74 | @STRING{iccvw = "Int. Conf. Comput. Vis. Workshops (ICCVW)" } 75 | @STRING{eccv = "Eur. Conf. Comput. Vis. (ECCV)" } 76 | @STRING{bmvc = "British Machine Vis. Conf. (BMVC)" } 77 | @STRING{icpr = "{IEEE} Int. Conf. Pattern Recog. (ICPR)" } 78 | @STRING{icvs = "Int. Conf. Comput. Vis. Syst. (ICVS)" } 79 | @STRING{crv = "Conf. Comput. Robot Vis. (CRV)" } 80 | @STRING{isvc = "Int. Symp. Adv. Vis. Comput. (ISVC)" } 81 | @STRING{threedv = "3D Vision (3DV)" } 82 | % - artificial intelligence 83 | @STRING{ijcai = "Int. Joint Conf. Artificial Intell. (IJCAI)" } 84 | @STRING{aaai = "{AAAI} Conf. Artificial Intell." } 85 | @STRING{siggraph= "SIGGRAPH" } 86 | @STRING{fusion = "Int. Conf. Inf. Fusion (FUSION)" } 87 | @STRING{ijcnn = "Int. Joint Conf. Neural Netw. (IJCNN)" } 88 | @STRING{ivs = "{IEEE} Intell. Vehicles Symp." } 89 | % - control 90 | @STRING{cdc = "{IEEE} Conf. Decision Control (CDC)" } 91 | @STRING{cca = "{IEEE} Conf. Control Appl. (CCA)" } 92 | @STRING{ecc = "{IEEE} Eur. Control Conf. (ECC)" } 93 | @STRING{ccc = "{IEEE} Chin. Control Conf. (CCC)" } 94 | @STRING{acc = "{IEEE} Am. Control Conf. (ACC)" } 95 | @STRING{ccta = "{IEEE} Conf. Cont. Tech. Applications (CCTA)" } 96 | @STRING{acra = "Australasian Conf. Robot. Autom." } 97 | % - misc 98 | @STRING{iscas = "{IEEE} Int. Symp. Circuits Syst. (ISCAS)" } 99 | @STRING{isca = "{ACM/IEEE} Int. Symp. Comp. Arch. (ISCA)" } 100 | @STRING{icann = "Int. Conf. Artificial Neural Netw." } 101 | @STRING{sac = "{ACM} Symp. Applied Computing (SAC)" } 102 | @STRING{icassp = "Int. Conf. Acoust., Speech, Signal Proc. (ICASSP)" } 103 | @STRING{isairas = "Int. Symp. Artificial Intell., Robot. Autom. in Space 104 | (iSAIRAS)" } 105 | @STRING{ebccsp = "Int. Conf. Event-Based Control, Comm. Signal Proc. (EBCCSP)" } 106 | @STRING{isscc = "{IEEE} Intl. Solid-State Circuits Conf. (ISSCC)" } 107 | @STRING{iccp = "{IEEE} Int. Conf. Comput. Photography (ICCP)" } 108 | @STRING{isvlsi = "{IEEE} Comp. Soc. Symp. VLSI (ISVLSI)" } 109 | % - misc workshops 110 | @STRING{ismar = "{IEEE} ACM Int. Sym. Mixed and Augmented Reality (ISMAR)" } 111 | @STRING{icraoss = "{ICRA} Workshop Open Source Softw." } 112 | @STRING{wpnc = "Workshop Positioning, Navigation Comm." } 113 | % PUBLISHERS 114 | @STRING{mit = "The MIT Press, Cambridge, MA" } 115 | @STRING{arxiv = "ar{X}iv e-prints" } 116 | 117 | @TechReport{Brescianini13tr, 118 | author = {Dario Brescianini and Markus Hehn and Raffaello D'Andrea}, 119 | title = {Nonlinear Quadrocopter Attitude Control}, 120 | institution = {Department of Mechanical and Process Engineering, ETHZ}, 121 | year = 2013, 122 | month = oct 123 | } 124 | 125 | @InProceedings{Faessler15icra, 126 | author = {Matthias Faessler and Flavio Fontana and Christian Forster 127 | and Davide Scaramuzza}, 128 | title = {Automatic Re-Initialization and Failure Recovery for 129 | Aggressive Flight with a Monocular Vision-Based Quadrotor}, 130 | booktitle = icra, 131 | year = 2015, 132 | pages = {1722--1729}, 133 | doi = {10.1109/ICRA.2015.7139420} 134 | } 135 | 136 | @Article{Faessler16jfr, 137 | author = {Matthias Faessler and Flavio Fontana and Christian Forster 138 | and Elias Mueggler and Matia Pizzoli and Davide Scaramuzza}, 139 | title = {Autonomous, Vision-based Flight and Live Dense {3D} Mapping 140 | with a Quadrotor {MAV}}, 141 | journal = jfr, 142 | year = 2016, 143 | volume = 33, 144 | number = 4, 145 | pages = {431--450}, 146 | issn = {1556-4967}, 147 | doi = {10.1002/rob.21581} 148 | } 149 | 150 | @Article{Faessler17ral, 151 | author = {Matthias Faessler and Davide Falanga and Davide Scaramuzza}, 152 | title = {Thrust Mixing, Saturation, and Body-Rate Control for Accurate 153 | Aggressive Quadrotor Flight}, 154 | journal = ral, 155 | year = 2017, 156 | volume = 2, 157 | number = 2, 158 | pages = {476--482}, 159 | month = apr, 160 | doi = {10.1109/LRA.2016.2640362}, 161 | issn = {2377-3766} 162 | } 163 | 164 | @Article{Faessler17tr, 165 | author = {Matthias Faessler and Antonio Franchi and Davide Scaramuzza}, 166 | title = {Detailed Derivations of \lq {D}ifferential Flatness of 167 | Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking 168 | of High-Speed Trajectories\rq}, 169 | journal = arxiv, 170 | year = 2017, 171 | month = dec, 172 | primaryclass = {cs.RO}, 173 | url = {http://arxiv.org/abs/1712.02402}, 174 | arxivid = {1712.02402} 175 | } 176 | 177 | @Article{Faessler18ral, 178 | author = {Matthias Faessler and Antonio Franchi and Davide Scaramuzza}, 179 | title = {Differential Flatness of Quadrotor Dynamics Subject to Rotor 180 | Drag for Accurate Tracking of High-Speed Trajectories}, 181 | journal = ral, 182 | year = 2018, 183 | volume = 3, 184 | number = 2, 185 | pages = {620--626}, 186 | month = apr, 187 | doi = {10.1109/LRA.2017.2776353}, 188 | issn = {2377-3766} 189 | } 190 | 191 | @InProceedings{Kai17ifac, 192 | author = {Jean-Marie Kai and Guillaume Allibert and Minh-Duc Hua and 193 | Tarek Hamel}, 194 | title = {Nonlinear feedback control of Quadrotors exploiting 195 | First-Order Drag Effects}, 196 | booktitle = ifac, 197 | year = 2017, 198 | volume = 50, 199 | pages = {8189--8195}, 200 | month = jul, 201 | number = 1, 202 | doi = {10.1016/j.ifacol.2017.08.1267} 203 | } 204 | 205 | @InProceedings{Lee10cdc, 206 | author = {Lee, T. and Leoky, M. and McClamroch, N.H.}, 207 | title = {Geometric tracking control of a quadrotor UAV on SE(3)}, 208 | booktitle = cdc, 209 | year = 2010, 210 | pages = {5420--5425}, 211 | month = dec, 212 | doi = {10.1109/CDC.2010.5717652}, 213 | issn = {0743-1546} 214 | } 215 | 216 | @Article{Lupashin14mech, 217 | author = {Sergei Lupashin and Markus Hehn and Mark W. Mueller and 218 | Angela P. Schoellig and Michael Sherback and Raffaello 219 | D’Andrea}, 220 | title = {A platform for aerial robotics research and demonstration: 221 | The {F}lying {M}achine {A}rena}, 222 | journal = mech, 223 | year = 2014, 224 | volume = 24, 225 | number = 1, 226 | pages = {41--54}, 227 | month = feb, 228 | doi = {10.1016/j.mechatronics.2013.11.006} 229 | } 230 | 231 | @Article{Mayhew11ac, 232 | author = {Christopher G. Mayhew and Ricardo G. Sanfelice and Andrew R. 233 | Teel}, 234 | title = {Quaternion-Based Hybrid Control for Robust Global Attitude 235 | Tracking}, 236 | journal = ac, 237 | year = 2011, 238 | volume = 56, 239 | number = 11, 240 | pages = {2555--2566}, 241 | month = nov, 242 | doi = {10.1109/tac.2011.2108490} 243 | } 244 | 245 | @InProceedings{Mellinger11icra, 246 | author = {Daniel Mellinger and Vijay Kumar}, 247 | title = {Minimum snap trajectory generation and control for 248 | quadrotors}, 249 | booktitle = icra, 250 | year = 2011, 251 | pages = {2520--2525}, 252 | month = may, 253 | doi = {10.1109/ICRA.2011.5980409}, 254 | issn = {1050-4729} 255 | } 256 | 257 | @Article{Michael10ram, 258 | author = {Nathan Michael and Daniel Mellinger and Quentin Lindsey and 259 | Vijay Kumar}, 260 | title = {The {GRASP} multiple micro {UAV} testbed}, 261 | journal = ram, 262 | year = 2010, 263 | volume = 17, 264 | number = 3, 265 | pages = {56--65}, 266 | month = sep 267 | } 268 | 269 | @InProceedings{Podhradsky13icuas, 270 | author = {Michal Podhradsky and Jarret Bone and Calvin Coopmans and 271 | Austin Jensen}, 272 | title = {Battery model-based thrust controller for a small, low cost 273 | multirotor Unmanned Aerial Vehicles}, 274 | booktitle = icuas, 275 | year = 2013, 276 | pages = {105--113}, 277 | month = may, 278 | doi = {10.1109/icuas.2013.6564679} 279 | } 280 | 281 | @InProceedings{Svacha17icuas, 282 | author = {James Svacha and Kartik Mohta and Vijay Kumar}, 283 | title = {Improving quadrotor trajectory tracking by compensating for 284 | aerodynamic effects}, 285 | booktitle = icuas, 286 | year = 2017, 287 | pages = {860--866}, 288 | month = jun, 289 | doi = {10.1109/icuas.2017.7991501} 290 | } 291 | 292 | @TechReport{Trawny05tr, 293 | author = {Trawny, N. and Roumeliotis, S. I.}, 294 | title = {Indirect {K}alman Filter for {3D} Attitude Estimation}, 295 | institution = {University of Minnesota, Dept. of Comp. Sci. \& Eng.}, 296 | year = 2005, 297 | number = {2005-002}, 298 | month = mar, 299 | priority = 2 300 | } 301 | --------------------------------------------------------------------------------