├── airo_control ├── logs │ └── .gitkeep ├── .gitignore ├── config │ ├── vicon │ │ ├── thrust_model.yaml │ │ ├── backstepping_vicon.yaml │ │ ├── slidingmode_vicon.yaml │ │ ├── mpc_vicon.yaml │ │ └── fsm_vicon.yaml │ └── gazebo │ │ ├── backstepping_gazebo.yaml │ │ ├── slidingmode_gazebo.yaml │ │ ├── mpc_gazebo.yaml │ │ └── fsm_gazebo.yaml ├── acados_scripts │ ├── c_generated_code │ │ ├── libacados_ocp_solver_quadrotor.so │ │ ├── quadrotor_cost │ │ │ ├── quadrotor_cost_y_0_fun.o │ │ │ ├── quadrotor_cost_y_e_fun.o │ │ │ ├── quadrotor_cost_y_fun.o │ │ │ ├── quadrotor_cost_y_hess.o │ │ │ ├── quadrotor_cost_y_0_hess.o │ │ │ ├── quadrotor_cost_y_e_hess.o │ │ │ ├── quadrotor_cost_y_0_fun_jac_ut_xt.o │ │ │ ├── quadrotor_cost_y_e_fun_jac_ut_xt.o │ │ │ ├── quadrotor_cost_y_fun_jac_ut_xt.o │ │ │ ├── quadrotor_cost_y_e_hess.c │ │ │ ├── quadrotor_cost_y_hess.c │ │ │ ├── quadrotor_cost_y_0_hess.c │ │ │ ├── quadrotor_cost_y_e_fun.c │ │ │ ├── quadrotor_cost_y_fun.c │ │ │ ├── quadrotor_cost_y_0_fun.c │ │ │ ├── quadrotor_cost.h │ │ │ ├── quadrotor_cost_y_e_fun_jac_ut_xt.c │ │ │ ├── quadrotor_cost_y_fun_jac_ut_xt.c │ │ │ └── quadrotor_cost_y_0_fun_jac_ut_xt.c │ │ ├── quadrotor_model │ │ │ ├── quadrotor_expl_ode_fun.o │ │ │ ├── quadrotor_expl_vde_adj.o │ │ │ ├── quadrotor_expl_vde_forw.o │ │ │ ├── quadrotor_model.h │ │ │ ├── quadrotor_expl_ode_fun.c │ │ │ ├── quadrotor_expl_vde_adj.c │ │ │ └── quadrotor_expl_vde_forw.c │ │ ├── quadrotor_constraints │ │ │ └── quadrotor_constraints.h │ │ └── acados_solver_quadrotor.h │ └── quadrotor_model.py ├── src │ ├── airo_control_node.cpp │ ├── controller │ │ ├── base_controller.cpp │ │ ├── backstepping.cpp │ │ ├── slidingmode.cpp │ │ └── mpc.cpp │ ├── rc_input.cpp │ ├── example_mission_node.cpp │ └── system_identification.cpp ├── launch │ ├── vicon │ │ ├── system_id_vicon.launch │ │ ├── fsm_vicon.launch │ │ ├── vrpn.launch │ │ └── mavros_vicon.launch │ └── gazebo │ │ ├── system_id_gazebo.launch │ │ ├── fsm_gazebo.launch │ │ └── mavros_gazebo.launch ├── include │ └── airo_control │ │ ├── controller │ │ ├── backstepping.h │ │ ├── slidingmode.h │ │ ├── base_controller.h │ │ └── mpc.h │ │ ├── rc_input.h │ │ └── airo_control_fsm.h ├── package.xml └── CMakeLists.txt ├── media ├── AIRo_PX4_FSM.png └── AIRo_PX4_FSM.drawio ├── startup ├── wait_for_ros.sh ├── vicon.yml ├── vicon_start.sh ├── gazebo_start.sh └── gazebo.yml ├── .gitmodules ├── .gitignore └── README.md /airo_control/logs/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /airo_control/.gitignore: -------------------------------------------------------------------------------- 1 | /logs/*.csv -------------------------------------------------------------------------------- /media/AIRo_PX4_FSM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/media/AIRo_PX4_FSM.png -------------------------------------------------------------------------------- /startup/wait_for_ros.sh: -------------------------------------------------------------------------------- 1 | until rostopic list > /dev/null 2>&1; do 2 | echo "waiting for ros" 3 | sleep 1; 4 | done -------------------------------------------------------------------------------- /airo_control/config/vicon/thrust_model.yaml: -------------------------------------------------------------------------------- 1 | mass: 0.711 # kg 2 | K1 : 0.211221921558914 3 | K2 : 1.887681577434585 4 | K3 : 0.559194070136690 -------------------------------------------------------------------------------- /airo_control/config/gazebo/backstepping_gazebo.yaml: -------------------------------------------------------------------------------- 1 | pub_debug: true 2 | 3 | hover_thrust: 0.5656428200006485 4 | k_x1: 1.0 5 | k_x2: 1.0 6 | k_y1: 1.0 7 | k_y2: 1.0 8 | k_z1: 5.0 9 | k_z2: 1.0 -------------------------------------------------------------------------------- /airo_control/config/vicon/backstepping_vicon.yaml: -------------------------------------------------------------------------------- 1 | pub_debug: false 2 | enable_thrust_model: true 3 | 4 | hover_thrust: 0.31 5 | k_x1: 1.0 6 | k_x2: 1.0 7 | k_y1: 1.0 8 | k_y2: 1.0 9 | k_z1: 5.0 10 | k_z2: 1.0 -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/libacados_ocp_solver_quadrotor.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/libacados_ocp_solver_quadrotor.so -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_hess.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_hess.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_hess.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_hess.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_hess.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_hess.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_ode_fun.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_ode_fun.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_vde_adj.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_vde_adj.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_vde_forw.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_vde_forw.o -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "airo_message"] 2 | path = airo_message 3 | url = https://github.com/pattylo/airo_message.git 4 | [submodule "airo_trajectory"] 5 | path = airo_trajectory 6 | url = https://github.com/HKPolyU-UAV/airo_trajectory.git 7 | -------------------------------------------------------------------------------- /airo_control/config/gazebo/slidingmode_gazebo.yaml: -------------------------------------------------------------------------------- 1 | pub_debug: true 2 | 3 | hover_thrust: 0.5656428200006485 4 | k_xe: 2.0 5 | k_xs: 1.0 6 | k_xt: 5.0 7 | k_ye: 2.0 8 | k_ys: 1.0 9 | k_yt: 5.0 10 | k_ze: 8.0 11 | k_zs: 1.0 12 | k_zt: 3.0 -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun_jac_ut_xt.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun_jac_ut_xt.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun_jac_ut_xt.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun_jac_ut_xt.o -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun_jac_ut_xt.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKPolyU-UAV/airo_control_interface/HEAD/airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun_jac_ut_xt.o -------------------------------------------------------------------------------- /airo_control/config/vicon/slidingmode_vicon.yaml: -------------------------------------------------------------------------------- 1 | show_debug: false 2 | enable_thrust_model: true 3 | 4 | hover_thrust: 0.31 5 | k_xe: 2.0 6 | k_xs: 1.0 7 | k_xt: 5.0 8 | k_ye: 2.0 9 | k_ys: 1.0 10 | k_yt: 5.0 11 | k_ze: 3.0 12 | k_zs: 2.5 13 | k_zt: 3.0 -------------------------------------------------------------------------------- /startup/vicon.yml: -------------------------------------------------------------------------------- 1 | name: airo_control_interface 2 | root: ./ 3 | startup_window: control_interface 4 | windows: 5 | - vicon: 6 | layout: tiled 7 | panes: 8 | - roslaunch airo_control mavros_vicon.launch 9 | - ./wait_for_ros.sh; roslaunch airo_control vrpn.launch 10 | - ./wait_for_ros.sh; roslaunch airo_control fsm_vicon.launch 11 | - ./wait_for_ros.sh; -------------------------------------------------------------------------------- /startup/vicon_start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | # remove the old link 10 | rm .tmuxinator.yml 11 | 12 | # link the session file to .tmuxinator.yml 13 | ln vicon.yml .tmuxinator.yml 14 | 15 | # start tmuxinator 16 | #tmuxinator 17 | tmuxinator start -p vicon.yml -------------------------------------------------------------------------------- /startup/gazebo_start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | # remove the old link 10 | rm .tmuxinator.yml 11 | 12 | # link the session file to .tmuxinator.yml 13 | ln gazebo.yml .tmuxinator.yml 14 | 15 | # start tmuxinator 16 | #tmuxinator 17 | tmuxinator start -p gazebo.yml -------------------------------------------------------------------------------- /airo_control/src/airo_control_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "airo_control/airo_control_fsm.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "airo_control_node"); 8 | ros::NodeHandle nh; 9 | int FSM_FREQUENCY; 10 | nh.getParam("airo_control_node/fsm/fsm_frequency",FSM_FREQUENCY); 11 | ros::Rate loop_rate(FSM_FREQUENCY); 12 | AIRO_CONTROL_FSM fsm(nh); 13 | 14 | while(ros::ok()){ 15 | ros::spinOnce(); 16 | fsm.process(); 17 | loop_rate.sleep(); 18 | } 19 | 20 | return 0; 21 | } -------------------------------------------------------------------------------- /startup/gazebo.yml: -------------------------------------------------------------------------------- 1 | name: airo_control_interface 2 | root: ./ 3 | startup_window: control_interface 4 | windows: 5 | - roscore: 6 | layout: tiled 7 | panes: 8 | - roscore 9 | - ./wait_for_ros.sh; cd ~/PX4-Autopilot; 10 | make px4_sitl_default gazebo 11 | - ./wait_for_ros.sh; roslaunch airo_control mavros_gazebo.launch 12 | - control_interface: 13 | layout: tiled 14 | panes: 15 | - ./wait_for_ros.sh; roslaunch airo_control fsm_gazebo.launch 16 | - ./wait_for_ros.sh; rosrun airo_control example_mission_node -------------------------------------------------------------------------------- /airo_control/config/gazebo/mpc_gazebo.yaml: -------------------------------------------------------------------------------- 1 | pub_debug: true 2 | enable_preview: true 3 | 4 | # system parameters 5 | hover_thrust: 0.5656428200006485 6 | tau_phi: 0.1742504945971047 7 | tau_theta: 0.1704274688906173 8 | tau_psi: 0.7410010081424614 9 | 10 | # costs 11 | diag_cost_x: 12 | - 80.0 13 | - 80.0 14 | - 150.0 15 | - 80.0 16 | - 80.0 17 | - 120.0 18 | - 10.0 19 | - 10.0 20 | diag_cost_u: 21 | - 20.0 22 | - 1000.0 23 | - 1000.0 24 | diag_cost_xn: 25 | - 80.0 26 | - 80.0 27 | - 150.0 28 | - 80.0 29 | - 80.0 30 | - 120.0 31 | - 10.0 32 | - 10.0 -------------------------------------------------------------------------------- /airo_control/launch/vicon/system_id_vicon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /airo_control/launch/gazebo/system_id_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /airo_control/launch/gazebo/fsm_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | airo_control/acados_scripts/__pycache__ 3 | airo_control/acados_scripts/acados_ocp.json 4 | airo_control/acados_scripts/c_generated_code/Makefile 5 | airo_control/acados_scripts/c_generated_code/acados_sim_solver_quadrotor.c 6 | airo_control/acados_scripts/c_generated_code/acados_sim_solver_quadrotor.h 7 | airo_control/acados_scripts/c_generated_code/acados_solver.pxd 8 | airo_control/acados_scripts/c_generated_code/main_quadrotor.c 9 | airo_control/acados_scripts/c_generated_code/main_sim_quadrotor.c 10 | airo_control/acados_scripts/c_generated_code/acados_solver_quadrotor.c 11 | airo_control/acados_scripts/c_generated_code/acados_solver_quadrotor.o 12 | startup/.tmuxinator.yml -------------------------------------------------------------------------------- /airo_control/launch/vicon/fsm_vicon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /airo_control/include/airo_control/controller/backstepping.h: -------------------------------------------------------------------------------- 1 | #ifndef BACKSTEPPING_H 2 | #define BACKSTEPPING_H 3 | 4 | #include 5 | 6 | #include "airo_control/controller/base_controller.h" 7 | 8 | class BACKSTEPPING : public BASE_CONTROLLER{ 9 | private: 10 | struct Param : public BASE_CONTROLLER::Param{ 11 | double k_x1,k_x2,k_y1,k_y2,k_z1,k_z2; 12 | }; 13 | 14 | double e_x1,e_x2,e_y1,e_y2,e_z1,e_z2,u_x,u_y,a_z; 15 | ros::Publisher debug_pub; 16 | 17 | public: 18 | Param param; 19 | BACKSTEPPING(ros::NodeHandle&); 20 | void pub_debug(); 21 | mavros_msgs::AttitudeTarget solve(const geometry_msgs::PoseStamped&, const geometry_msgs::TwistStamped&, const geometry_msgs::AccelStamped&, const airo_message::ReferenceStamped&, const sensor_msgs::BatteryState&); 22 | double get_hover_thrust(); 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /airo_control/launch/vicon/vrpn.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | server: $(arg vicon_server) 6 | port: 3883 7 | 8 | update_frequency: 100.0 9 | frame_id: world 10 | 11 | # Use the VRPN server's time, or the client's ROS time. 12 | use_server_time: false 13 | broadcast_tf: true 14 | 15 | # Must either specify refresh frequency > 0.0, or a list of trackers to create 16 | refresh_tracker_frequency: 1.0 17 | #trackers: 18 | #- FirstTracker 19 | #- SecondTracker 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /airo_control/include/airo_control/controller/slidingmode.h: -------------------------------------------------------------------------------- 1 | #ifndef SLIDINGMODE_H 2 | #define SLIDINGMODE_H 3 | 4 | #include 5 | 6 | #include "airo_control/controller/base_controller.h" 7 | 8 | class SLIDINGMODE : public BASE_CONTROLLER{ 9 | private: 10 | struct Param : public BASE_CONTROLLER::Param{ 11 | double k_xe,k_xs,k_xt,k_ye,k_ys,k_yt,k_ze,k_zs,k_zt; 12 | }; 13 | 14 | double e_x,e_dx,s_x,e_y,e_dy,s_y,e_z,e_dz,s_z,u_x,u_y,a_z; 15 | ros::Publisher debug_pub; 16 | 17 | public: 18 | Param param; 19 | SLIDINGMODE(ros::NodeHandle&); 20 | void pub_debug(); 21 | mavros_msgs::AttitudeTarget solve(const geometry_msgs::PoseStamped&, const geometry_msgs::TwistStamped&, const geometry_msgs::AccelStamped&, const airo_message::ReferenceStamped&, const sensor_msgs::BatteryState&); 22 | double get_hover_thrust(); 23 | int sign(double&); 24 | }; 25 | 26 | #endif -------------------------------------------------------------------------------- /airo_control/config/vicon/mpc_vicon.yaml: -------------------------------------------------------------------------------- 1 | pub_debug: false 2 | enable_thrust_model: true 3 | 4 | # system parameters 5 | hover_thrust: 0.31 6 | enable_preview: true 7 | tau_phi: 0.1520336055355853 8 | tau_theta: 0.1413976464134012 9 | tau_psi: 0.540818835503479 10 | 11 | # # conservative costs 12 | # diag_cost_x: 13 | # - 60.0 14 | # - 60.0 15 | # - 60.0 16 | # - 30.0 17 | # - 30.0 18 | # - 30.0 19 | # - 10.0 20 | # - 10.0 21 | 22 | # diag_cost_u: 23 | # - 200.0 24 | # - 1000.0 25 | # - 1000.0 26 | 27 | # diag_cost_xn: 28 | # - 60.0 29 | # - 60.0 30 | # - 60.0 31 | # - 30.0 32 | # - 30.0 33 | # - 30.0 34 | # - 10.0 35 | # - 10.0 36 | 37 | # aggressive costs 38 | diag_cost_x: 39 | - 80.0 40 | - 80.0 41 | - 150.0 42 | - 80.0 43 | - 80.0 44 | - 120.0 45 | - 10.0 46 | - 10.0 47 | 48 | diag_cost_u: 49 | - 20.0 50 | - 1000.0 51 | - 1000.0 52 | 53 | diag_cost_xn: 54 | - 80.0 55 | - 80.0 56 | - 150.0 57 | - 80.0 58 | - 80.0 59 | - 120.0 60 | - 10.0 61 | - 10.0 -------------------------------------------------------------------------------- /airo_control/config/vicon/fsm_vicon.yaml: -------------------------------------------------------------------------------- 1 | # ROS Topics 2 | pose_topic: /mavros/vision_pose/pose 3 | twist_topic: /mavros/local_position/velocity_local 4 | 5 | # FSM Parameters 6 | # controller_type: mpc 7 | controller_type: backstepping 8 | # controller_type: slidingmode 9 | enable_logging: false 10 | fsm_frequency: 100 11 | state_timeout: 1.5 12 | rc_timeout: 0.25 13 | odom_timeout: 0.5 14 | command_timeout: 0.25 15 | motor_speedup_time: 5.0 16 | takeoff_height: 1.0 17 | takeoff_land_speed: 0.3 18 | reject_takeoff_twist_threshold: 0.10 19 | hover_max_velocity: 1.5 20 | hover_max_yaw_rate: 1.5 21 | check_safety_volumn: true 22 | safety_volumn: [-2.3, 2.7, -1.7, 2.3, 2.5] 23 | 24 | # RC Transmitter Parameters 25 | without_rc: false 26 | throttle_channel: 3 27 | yaw_channel: 4 28 | pitch_channel: 2 29 | roll_channel: 1 30 | fsm_channel: 6 31 | command_channel: 7 32 | reboot_channel: 9 33 | kill_channel: 8 34 | reverse_throttle: true 35 | reverse_yaw: false 36 | reverse_pitch: true 37 | reverse_roll: false 38 | switch_threshold: 0.75 39 | joystick_deadzone: 0.10 40 | check_centered_threshold: 1.0 -------------------------------------------------------------------------------- /airo_control/config/gazebo/fsm_gazebo.yaml: -------------------------------------------------------------------------------- 1 | # ROS Topics 2 | pose_topic: /mavros/local_position/pose 3 | twist_topic: /mavros/local_position/velocity_local 4 | 5 | # FSM Parameters 6 | controller_type: mpc 7 | # controller_type: backstepping 8 | # controller_type: slidingmode 9 | enable_logging: false 10 | fsm_frequency: 100 11 | state_timeout: 1.5 12 | rc_timeout: 0.25 13 | odom_timeout: 0.5 14 | command_timeout: 0.25 15 | motor_speedup_time: 5.0 16 | takeoff_height: 1.0 17 | takeoff_land_speed: 0.3 18 | reject_takeoff_twist_threshold: 0.6 19 | hover_max_velocity: 3.0 20 | hover_max_yaw_rate: 3.0 21 | check_safety_volumn: true 22 | safety_volumn: [-20.0, 20.0, -20.0, 20.0, 10.0] 23 | 24 | # RC Transmitter Parameters 25 | without_rc: true 26 | throttle_channel: 4 27 | yaw_channel: 3 28 | pitch_channel: 1 29 | roll_channel: 2 30 | fsm_channel: 5 31 | command_channel: 7 32 | reboot_channel: 9 33 | kill_channel: 8 34 | reverse_throttle: false 35 | reverse_yaw: false 36 | reverse_pitch: false 37 | reverse_roll: false 38 | switch_threshold: 0.75 39 | joystick_deadzone: 0.10 40 | check_centered_threshold: 0.3 -------------------------------------------------------------------------------- /airo_control/launch/gazebo/mavros_gazebo.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 | -------------------------------------------------------------------------------- /airo_control/launch/vicon/mavros_vicon.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 | -------------------------------------------------------------------------------- /airo_control/src/controller/base_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "airo_control/controller/base_controller.h" 2 | 3 | Eigen::Vector3d BASE_CONTROLLER::q2rpy(const geometry_msgs::Quaternion& quaternion){ 4 | tf::Quaternion tf_quaternion; 5 | Eigen::Vector3d euler; 6 | tf::quaternionMsgToTF(quaternion,tf_quaternion); 7 | tf::Matrix3x3(tf_quaternion).getRPY(euler.x(), euler.y(), euler.z()); 8 | return euler; 9 | } 10 | 11 | geometry_msgs::Quaternion BASE_CONTROLLER::rpy2q(const Eigen::Vector3d& euler){ 12 | geometry_msgs::Quaternion quaternion = tf::createQuaternionMsgFromRollPitchYaw(euler.x(), euler.y(), euler.z()); 13 | return quaternion; 14 | } 15 | 16 | float BASE_CONTROLLER::inverse_thrust_model(const double& a_z,const float& voltage,const Param& param,const ThrustModel& thrust_model){ 17 | float thrust; 18 | 19 | if (param.enable_thrust_model) { 20 | thrust = ((sqrt((thrust_model.mass*a_z)/(thrust_model.K1*pow(voltage,thrust_model.K2))+pow(((1-thrust_model.K3)/(2*sqrt(thrust_model.K3))),2))-((1-thrust_model.K3)/(2*sqrt(thrust_model.K3))))/sqrt(thrust_model.K3)); 21 | } 22 | else { 23 | thrust = (a_z/g)*param.hover_thrust; 24 | } 25 | 26 | if (thrust > 1.0) { 27 | ROS_ERROR("Thrust = %f",thrust); 28 | thrust = 1.0; 29 | } 30 | else if (thrust < 0.0) { 31 | thrust = 0.0; 32 | } 33 | 34 | return thrust; 35 | } -------------------------------------------------------------------------------- /airo_control/include/airo_control/rc_input.h: -------------------------------------------------------------------------------- 1 | #ifndef RC_INPUT_H 2 | #define RC_INPUT_H 3 | 4 | #include 5 | #include "mavros_msgs/RCIn.h" 6 | 7 | class RC_INPUT{ 8 | public: 9 | 10 | struct RC_PARAM{ 11 | int THROTTLE_CHANNEL; 12 | int YAW_CHANNEL; 13 | int PITCH_CHANNEL; 14 | int ROLL_CHANNEL; 15 | int FSM_CHANNEL; 16 | int COMMAND_CHANNEL; 17 | int REBOOT_CHANNEL; 18 | int KILL_CHANNEL; 19 | bool REVERSE_THROTTLE; 20 | bool REVERSE_YAW; 21 | bool REVERSE_PITCH; 22 | bool REVERSE_ROLL; 23 | double SWITCH_THRESHOLD; 24 | double JOYSTICK_DEADZONE; 25 | double CHECK_CENTERED_THRESHOLD; 26 | }; 27 | 28 | ros::Time stamp; 29 | RC_PARAM rc_param; 30 | double channel[4]; // Pitch, Roll, Yaw, and Thrust mapped from 1000~2000 to -1~1 31 | double fsm_switch; // Switch position mapped from 0~2000 to -1~1 32 | double last_fsm_switch; 33 | double command_switch; // Switch position mapped from 0~2000 to -1~1 34 | double reboot_switch; // Switch position mapped from 0~2000 to -1~1 35 | double last_reboot_switch; 36 | double kill_switch; 37 | bool is_fsm; 38 | bool enter_fsm; 39 | bool is_command; 40 | bool enter_reboot; 41 | bool is_killed; 42 | bool init_indicator; 43 | 44 | RC_INPUT(); 45 | void check_validity(); 46 | bool check_centered(); 47 | void set_rc_param(RC_INPUT::RC_PARAM&); 48 | void process(const mavros_msgs::RCIn::ConstPtr&); 49 | }; 50 | 51 | #endif -------------------------------------------------------------------------------- /airo_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | airo_control 4 | 0.0.0 5 | The airo_control package 6 | 7 | 8 | 9 | 10 | rocky 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | 20 | geometry_msgs 21 | geometry_msgs 22 | geometry_msgs 23 | 24 | mavros_msgs 25 | mavros_msgs 26 | mavros_msgs 27 | 28 | roscpp 29 | roscpp 30 | roscpp 31 | 32 | std_msgs 33 | std_msgs 34 | std_msgs 35 | 36 | roslaunch 37 | roslaunch 38 | roslaunch 39 | 40 | airo_message 41 | airo_message 42 | airo_message 43 | 44 | 45 | -------------------------------------------------------------------------------- /airo_control/include/airo_control/controller/base_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef POSITION_CONTROLLER_H 2 | #define POSITION_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | class BASE_CONTROLLER{ 17 | protected: 18 | struct Param{ 19 | bool pub_debug = false; 20 | bool enable_thrust_model = false; 21 | double hover_thrust = 0.0; 22 | }; 23 | 24 | struct ThrustModel{ 25 | float mass; 26 | float K1; 27 | float K2; 28 | float K3; 29 | }; 30 | 31 | Param param; 32 | ThrustModel thrust_model; 33 | double g = 9.80665; 34 | Eigen::Vector3d ref_euler,current_euler,target_euler; 35 | mavros_msgs::AttitudeTarget attitude_target; 36 | 37 | public: 38 | virtual void pub_debug() = 0; 39 | virtual double get_hover_thrust() = 0; 40 | virtual mavros_msgs::AttitudeTarget solve(const geometry_msgs::PoseStamped&, const geometry_msgs::TwistStamped&, const geometry_msgs::AccelStamped&, const airo_message::ReferenceStamped&, const sensor_msgs::BatteryState&) = 0; 41 | Eigen::Vector3d q2rpy(const geometry_msgs::Quaternion&); 42 | geometry_msgs::Quaternion rpy2q(const Eigen::Vector3d&); 43 | float inverse_thrust_model(const double& a_z,const float& voltage,const Param& param,const ThrustModel& thrust_model); 44 | 45 | }; 46 | 47 | #endif -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_constraints/quadrotor_constraints.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, 3 | * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, 4 | * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, 5 | * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl 6 | * 7 | * This file is part of acados. 8 | * 9 | * The 2-Clause BSD License 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE.; 32 | */ 33 | 34 | #ifndef quadrotor_CONSTRAINTS 35 | #define quadrotor_CONSTRAINTS 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | #ifdef __cplusplus 50 | } /* extern "C" */ 51 | #endif 52 | 53 | #endif // quadrotor_CONSTRAINTS 54 | -------------------------------------------------------------------------------- /airo_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(airo_control) 3 | 4 | ## Compile as C++17, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | roslib 13 | std_msgs 14 | roslaunch 15 | geometry_msgs 16 | mavros_msgs 17 | tf 18 | airo_message 19 | ) 20 | 21 | find_package(Eigen3 REQUIRED) 22 | find_package(yaml-cpp REQUIRED) 23 | 24 | set(acados_include $ENV{HOME}/acados/include) 25 | set(acados_lib $ENV{HOME}/acados/lib) 26 | set(quadrotor_model ${PROJECT_SOURCE_DIR}/acados_scripts/c_generated_code) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | # LIBRARIES airo_control 31 | CATKIN_DEPENDS roscpp airo_message 32 | # DEPENDS system_lib 33 | ) 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ${quadrotor_model} 39 | ${acados_include} 40 | ${acados_include}/blasfeo/include/ 41 | ${acados_include}/hpipm/include/ 42 | ${acados_include}/acados/ 43 | ${acados_include}/qpOASES_e/ 44 | ) 45 | link_directories(${quadrotor_model}) 46 | 47 | add_executable(airo_control_node 48 | src/airo_control_node.cpp 49 | src/airo_control_fsm.cpp 50 | src/controller/base_controller.cpp 51 | src/controller/mpc.cpp 52 | src/controller/backstepping.cpp 53 | src/controller/slidingmode.cpp 54 | src/rc_input.cpp 55 | ) 56 | target_link_libraries(airo_control_node 57 | ${quadrotor_model}/libacados_ocp_solver_quadrotor.so 58 | ${acados_lib}/libacados.so 59 | ${catkin_LIBRARIES} 60 | ) 61 | add_dependencies(airo_control_node airo_message_generate_messages_cpp) 62 | 63 | add_executable(system_identification src/system_identification.cpp) 64 | target_link_libraries(system_identification 65 | ${catkin_LIBRARIES} 66 | yaml-cpp 67 | ) 68 | 69 | add_executable(example_mission_node src/example_mission_node.cpp) 70 | target_link_libraries(example_mission_node 71 | ${catkin_LIBRARIES} 72 | ) 73 | add_dependencies(example_mission_node airo_message_generate_messages_cpp) 74 | -------------------------------------------------------------------------------- /airo_control/include/airo_control/controller/mpc.h: -------------------------------------------------------------------------------- 1 | #ifndef MPC_H 2 | #define MPC_H 3 | 4 | #include 5 | 6 | #include "acados/utils/print.h" 7 | #include "acados_c/ocp_nlp_interface.h" 8 | #include "acados_c/external_function_interface.h" 9 | #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" 10 | #include "acados/ocp_nlp/ocp_nlp_cost_ls.h" 11 | 12 | #include "blasfeo/include/blasfeo_d_aux.h" 13 | #include "blasfeo/include/blasfeo_d_aux_ext_dep.h" 14 | 15 | #include "quadrotor_model/quadrotor_model.h" 16 | #include "acados_solver_quadrotor.h" 17 | 18 | #include "airo_control/controller/base_controller.h" 19 | #include "airo_message/ReferencePreview.h" 20 | 21 | class MPC : public BASE_CONTROLLER{ 22 | private: 23 | struct Param : public BASE_CONTROLLER::Param{ 24 | double tau_phi; 25 | double tau_theta; 26 | double tau_psi; 27 | bool enable_preview; 28 | std::vector diag_cost_x,diag_cost_u,diag_cost_xn; 29 | }; 30 | 31 | enum SystemStates{ 32 | x = 0, 33 | y = 1, 34 | z = 2, 35 | u = 3, 36 | v = 4, 37 | w = 5, 38 | phi = 6, 39 | theta = 7, 40 | psi = 8, 41 | }; 42 | 43 | enum ControlInputs{ 44 | thrust = 0, 45 | phi_cmd = 1, 46 | theta_cmd = 2, 47 | psi_cmd = 3, 48 | }; 49 | 50 | struct SolverInput{ 51 | double x0[QUADROTOR_NX]; 52 | double yref[QUADROTOR_N+1][QUADROTOR_NY]; 53 | }; 54 | 55 | struct SolverOutput{ 56 | double u0[QUADROTOR_NU]; 57 | double x1[QUADROTOR_NX]; 58 | double status,kkt_res, cpu_time; 59 | }; 60 | 61 | ros::Publisher debug_pub; 62 | 63 | // Acados variables 64 | SolverInput acados_in; 65 | SolverOutput acados_out; 66 | double acados_param[QUADROTOR_N+1][QUADROTOR_NP]; // hover_thrust, tau_phi, tau_theta, psi 67 | int acados_status; 68 | quadrotor_solver_capsule *mpc_capsule = quadrotor_acados_create_capsule(); 69 | 70 | public: 71 | Param param; 72 | MPC(ros::NodeHandle&); 73 | void pub_debug(); 74 | void print_debug(); 75 | double get_hover_thrust(); 76 | bool set_intermediate_weights(const std::vector&,const std::vector&); 77 | bool set_terminal_weights(const std::vector&); 78 | mavros_msgs::AttitudeTarget solve(const geometry_msgs::PoseStamped&, const geometry_msgs::TwistStamped&, const geometry_msgs::AccelStamped&, const airo_message::ReferenceStamped&, const sensor_msgs::BatteryState& battery_state); 79 | mavros_msgs::AttitudeTarget solve(const geometry_msgs::PoseStamped&, const geometry_msgs::TwistStamped&, const geometry_msgs::AccelStamped&, const airo_message::ReferencePreview&, const sensor_msgs::BatteryState& battery_state); 80 | }; 81 | 82 | #endif -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_model.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, 3 | * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, 4 | * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, 5 | * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl 6 | * 7 | * This file is part of acados. 8 | * 9 | * The 2-Clause BSD License 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE.; 32 | */ 33 | 34 | #ifndef quadrotor_MODEL 35 | #define quadrotor_MODEL 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | 42 | /* explicit ODE */ 43 | 44 | // explicit ODE 45 | int quadrotor_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 46 | int quadrotor_expl_ode_fun_work(int *, int *, int *, int *); 47 | const int *quadrotor_expl_ode_fun_sparsity_in(int); 48 | const int *quadrotor_expl_ode_fun_sparsity_out(int); 49 | int quadrotor_expl_ode_fun_n_in(void); 50 | int quadrotor_expl_ode_fun_n_out(void); 51 | 52 | // explicit forward VDE 53 | int quadrotor_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 54 | int quadrotor_expl_vde_forw_work(int *, int *, int *, int *); 55 | const int *quadrotor_expl_vde_forw_sparsity_in(int); 56 | const int *quadrotor_expl_vde_forw_sparsity_out(int); 57 | int quadrotor_expl_vde_forw_n_in(void); 58 | int quadrotor_expl_vde_forw_n_out(void); 59 | 60 | // explicit adjoint VDE 61 | int quadrotor_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 62 | int quadrotor_expl_vde_adj_work(int *, int *, int *, int *); 63 | const int *quadrotor_expl_vde_adj_sparsity_in(int); 64 | const int *quadrotor_expl_vde_adj_sparsity_out(int); 65 | int quadrotor_expl_vde_adj_n_in(void); 66 | int quadrotor_expl_vde_adj_n_out(void); 67 | 68 | 69 | 70 | #ifdef __cplusplus 71 | } /* extern "C" */ 72 | #endif 73 | 74 | #endif // quadrotor_MODEL 75 | -------------------------------------------------------------------------------- /airo_control/src/rc_input.cpp: -------------------------------------------------------------------------------- 1 | #include "airo_control/rc_input.h" 2 | 3 | RC_INPUT::RC_INPUT(){ 4 | stamp = ros::Time(0); 5 | 6 | last_fsm_switch = -1; 7 | last_reboot_switch = -1; 8 | 9 | // Important for no RC flight 10 | is_fsm = true; 11 | enter_fsm = false; 12 | is_command = true; 13 | enter_reboot = false; 14 | is_killed = false; 15 | 16 | for (int i = 0; i < 4; ++i){ 17 | channel[i] = 0.0; 18 | } 19 | } 20 | 21 | void RC_INPUT::process(const mavros_msgs::RCIn::ConstPtr& msg){ 22 | stamp = ros::Time::now(); 23 | 24 | for (int i = 0; i < 4; ++i){ 25 | channel[i] = ((double)msg->channels[i] - 1500.0) / 500.0; 26 | if (channel[i] > rc_param.JOYSTICK_DEADZONE) 27 | channel[i] = (channel[i] - rc_param.JOYSTICK_DEADZONE) / (1 - rc_param.JOYSTICK_DEADZONE); 28 | else if (channel[i] < -rc_param.JOYSTICK_DEADZONE) 29 | channel[i] = (channel[i] + rc_param.JOYSTICK_DEADZONE) / (1 - rc_param.JOYSTICK_DEADZONE); 30 | else 31 | channel[i] = 0; 32 | } 33 | 34 | fsm_switch = ((double)msg->channels[rc_param.FSM_CHANNEL - 1] - 1000.0) / 1000.0; 35 | command_switch = ((double)msg->channels[rc_param.COMMAND_CHANNEL - 1] - 1000.0) / 1000.0; 36 | reboot_switch = ((double)msg->channels[rc_param.REBOOT_CHANNEL - 1] - 1000.0) / 1000.0; 37 | kill_switch = ((double)msg->channels[rc_param.KILL_CHANNEL - 1] - 1000.0) / 1000.0; 38 | 39 | check_validity(); 40 | 41 | // Init last indicators for first run 42 | if (!init_indicator){ 43 | init_indicator = true; 44 | last_fsm_switch = fsm_switch; 45 | last_reboot_switch = reboot_switch; 46 | } 47 | 48 | // Set fsm 49 | if (last_fsm_switch < rc_param.SWITCH_THRESHOLD && fsm_switch > rc_param.SWITCH_THRESHOLD) 50 | enter_fsm = true; 51 | else 52 | enter_fsm = false; 53 | 54 | if (fsm_switch > rc_param.SWITCH_THRESHOLD) 55 | is_fsm = true; 56 | else 57 | is_fsm = false; 58 | 59 | // Set command 60 | if (command_switch > rc_param.SWITCH_THRESHOLD) 61 | is_command = true; 62 | else 63 | is_command = false; 64 | 65 | // Set reboot 66 | 67 | if (last_reboot_switch < rc_param.SWITCH_THRESHOLD && reboot_switch > rc_param.SWITCH_THRESHOLD) 68 | enter_reboot = true; 69 | else 70 | enter_reboot = false; 71 | 72 | // Set kill 73 | if (kill_switch > rc_param.SWITCH_THRESHOLD) 74 | is_killed = true; 75 | else 76 | is_killed = false; 77 | 78 | // Update last indicators 79 | last_fsm_switch = fsm_switch; 80 | last_reboot_switch = reboot_switch; 81 | } 82 | 83 | void RC_INPUT::check_validity(){ 84 | if (fsm_switch >= -1.1 && fsm_switch <= 1.1 && command_switch >= -1.1 && command_switch <= 1.1 && reboot_switch >= -1.1 && reboot_switch <= 1.1) 85 | { 86 | // pass 87 | } 88 | else 89 | { 90 | ROS_ERROR("RC input validity check fail. fsm=%f, command=%f, reboot=%f", fsm_switch, command_switch, reboot_switch); 91 | } 92 | } 93 | 94 | bool RC_INPUT::check_centered(){ 95 | bool centered = abs(channel[0]) < rc_param.CHECK_CENTERED_THRESHOLD && abs(channel[1]) < rc_param.CHECK_CENTERED_THRESHOLD && abs(channel[2]) < rc_param.CHECK_CENTERED_THRESHOLD && abs(channel[3]) < rc_param.CHECK_CENTERED_THRESHOLD; 96 | return centered; 97 | } 98 | 99 | void RC_INPUT::set_rc_param(RC_INPUT::RC_PARAM& param){ 100 | rc_param = param; 101 | } -------------------------------------------------------------------------------- /airo_control/src/controller/backstepping.cpp: -------------------------------------------------------------------------------- 1 | #include "airo_control/controller/backstepping.h" 2 | 3 | BACKSTEPPING::BACKSTEPPING(ros::NodeHandle& nh){ 4 | // Get Parameters 5 | nh.getParam("airo_control_node/backstepping/pub_debug",param.pub_debug); 6 | nh.getParam("airo_control_node/backstepping/enable_thrust_model",param.enable_thrust_model); 7 | nh.getParam("airo_control_node/backstepping/hover_thrust",param.hover_thrust); 8 | nh.getParam("airo_control_node/backstepping/k_x1",param.k_x1); 9 | nh.getParam("airo_control_node/backstepping/k_x2",param.k_x2); 10 | nh.getParam("airo_control_node/backstepping/k_y1",param.k_y1); 11 | nh.getParam("airo_control_node/backstepping/k_y2",param.k_y2); 12 | nh.getParam("airo_control_node/backstepping/k_z1",param.k_z1); 13 | nh.getParam("airo_control_node/backstepping/k_z2",param.k_z2); 14 | 15 | if (param.enable_thrust_model){ 16 | nh.getParam("airo_control_node/thrust_model/mass",thrust_model.mass); 17 | nh.getParam("airo_control_node/thrust_model/K1",thrust_model.K1); 18 | nh.getParam("airo_control_node/thrust_model/K2",thrust_model.K2); 19 | nh.getParam("airo_control_node/thrust_model/K3",thrust_model.K3); 20 | } 21 | 22 | debug_pub = nh.advertise("/airo_control/backstepping/debug",1); 23 | } 24 | 25 | void BACKSTEPPING::pub_debug(){ 26 | std_msgs::Float64MultiArray debug_msg; 27 | debug_msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); 28 | debug_msg.layout.dim[0].size = 8; 29 | debug_msg.layout.dim[0].stride = 1; 30 | debug_msg.data.clear(); 31 | debug_msg.data.push_back(e_z1); 32 | debug_msg.data.push_back(e_z2); 33 | debug_msg.data.push_back(e_x1); 34 | debug_msg.data.push_back(e_x2); 35 | debug_msg.data.push_back(u_x); 36 | debug_msg.data.push_back(e_y1); 37 | debug_msg.data.push_back(e_y2); 38 | debug_msg.data.push_back(u_y); 39 | 40 | debug_pub.publish(debug_msg); 41 | } 42 | 43 | mavros_msgs::AttitudeTarget BACKSTEPPING::solve(const geometry_msgs::PoseStamped& current_pose, const geometry_msgs::TwistStamped& current_twist, const geometry_msgs::AccelStamped& current_accel, const airo_message::ReferenceStamped& ref, const sensor_msgs::BatteryState& battery_state){ 44 | current_euler = q2rpy(current_pose.pose.orientation); 45 | ref_euler = q2rpy(ref.ref.pose.orientation); 46 | 47 | // Altitude Control 48 | e_z1 = ref.ref.pose.position.z - current_pose.pose.position.z; 49 | e_z2 = current_twist.twist.linear.z - ref.ref.twist.linear.z - param.k_z1*e_z1; 50 | a_z = (e_z1+g+ref.ref.accel.linear.z-param.k_z1*(e_z2 + param.k_z1*e_z1)-param.k_z2*e_z2)/(cos(current_euler.x())*cos(current_euler.y())); 51 | attitude_target.thrust = inverse_thrust_model(a_z,battery_state.voltage,param,thrust_model); 52 | 53 | // X Translation Control 54 | e_x1 = ref.ref.pose.position.x - current_pose.pose.position.x; 55 | e_x2 = current_twist.twist.linear.x - ref.ref.twist.linear.x - param.k_x1*e_x1; 56 | u_x = (e_x1+ref.ref.accel.linear.x-param.k_x1*(e_x2+param.k_x1*e_x1)-param.k_x2*e_x2)/a_z; 57 | 58 | // Y Translation Control 59 | e_y1 = ref.ref.pose.position.y - current_pose.pose.position.y; 60 | e_y2 = current_twist.twist.linear.y - ref.ref.twist.linear.y - param.k_y1*e_y1; 61 | u_y = (e_y1+ref.ref.accel.linear.y-param.k_y1*(e_y2+param.k_y1*e_y1)-param.k_y2*e_y2)/a_z; 62 | 63 | // Calculate Targer Eulers 64 | target_euler.x() = asin(u_x*sin(ref_euler.z()) - u_y*cos(ref_euler.z())); 65 | target_euler.y() = asin((u_x*cos(ref_euler.z()) + u_y*sin(ref_euler.z()))/cos(target_euler.x())); 66 | target_euler.z() = ref_euler.z(); 67 | attitude_target.orientation = rpy2q(target_euler); 68 | 69 | if (param.pub_debug){ 70 | pub_debug(); 71 | } 72 | 73 | return attitude_target; 74 | } 75 | 76 | double BACKSTEPPING::get_hover_thrust(){ 77 | return param.hover_thrust; 78 | } -------------------------------------------------------------------------------- /airo_control/src/controller/slidingmode.cpp: -------------------------------------------------------------------------------- 1 | #include "airo_control/controller/slidingmode.h" 2 | 3 | SLIDINGMODE::SLIDINGMODE(ros::NodeHandle& nh){ 4 | // Get Parameters 5 | nh.getParam("airo_control_node/slidingmode/pub_debug",param.pub_debug); 6 | nh.getParam("airo_control_node/slidingmode/enable_thrust_model",param.enable_thrust_model); 7 | nh.getParam("airo_control_node/slidingmode/hover_thrust",param.hover_thrust); 8 | nh.getParam("airo_control_node/slidingmode/k_xe",param.k_xe); 9 | nh.getParam("airo_control_node/slidingmode/k_xs",param.k_xs); 10 | nh.getParam("airo_control_node/slidingmode/k_xt",param.k_xt); 11 | nh.getParam("airo_control_node/slidingmode/k_ye",param.k_ye); 12 | nh.getParam("airo_control_node/slidingmode/k_ys",param.k_ys); 13 | nh.getParam("airo_control_node/slidingmode/k_yt",param.k_yt); 14 | nh.getParam("airo_control_node/slidingmode/k_ze",param.k_ze); 15 | nh.getParam("airo_control_node/slidingmode/k_zs",param.k_zs); 16 | nh.getParam("airo_control_node/slidingmode/k_zt",param.k_zt); 17 | 18 | if (param.enable_thrust_model){ 19 | nh.getParam("airo_control_node/thrust_model/mass",thrust_model.mass); 20 | nh.getParam("airo_control_node/thrust_model/K1",thrust_model.K1); 21 | nh.getParam("airo_control_node/thrust_model/K2",thrust_model.K2); 22 | nh.getParam("airo_control_node/thrust_model/K3",thrust_model.K3); 23 | } 24 | 25 | debug_pub = nh.advertise("/airo_control/slidingmode/debug",1); 26 | } 27 | 28 | void SLIDINGMODE::pub_debug(){ 29 | std_msgs::Float64MultiArray debug_msg; 30 | debug_msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); 31 | debug_msg.layout.dim[0].size = 11; 32 | debug_msg.layout.dim[0].stride = 1; 33 | debug_msg.data.clear(); 34 | debug_msg.data.push_back(e_z); 35 | debug_msg.data.push_back(e_dz); 36 | debug_msg.data.push_back(s_z); 37 | debug_msg.data.push_back(e_x); 38 | debug_msg.data.push_back(e_dx); 39 | debug_msg.data.push_back(s_x); 40 | debug_msg.data.push_back(u_x); 41 | debug_msg.data.push_back(e_y); 42 | debug_msg.data.push_back(e_dy); 43 | debug_msg.data.push_back(s_y); 44 | debug_msg.data.push_back(u_y); 45 | 46 | debug_pub.publish(debug_msg); 47 | } 48 | 49 | mavros_msgs::AttitudeTarget SLIDINGMODE::solve(const geometry_msgs::PoseStamped& current_pose, const geometry_msgs::TwistStamped& current_twist, const geometry_msgs::AccelStamped& current_accel, const airo_message::ReferenceStamped& ref, const sensor_msgs::BatteryState& battery_state){ 50 | current_euler = q2rpy(current_pose.pose.orientation); 51 | ref_euler = q2rpy(ref.ref.pose.orientation); 52 | 53 | // Altitude Control 54 | e_z = ref.ref.pose.position.z - current_pose.pose.position.z; 55 | e_dz= ref.ref.twist.linear.z - current_twist.twist.linear.z; 56 | s_z = param.k_ze * e_z + e_dz; 57 | a_z = (param.k_ze*e_dz+ref.ref.accel.linear.z+g+param.k_zs*tanh(param.k_zt*s_z))/(cos(current_euler.x())*cos(current_euler.y())); 58 | attitude_target.thrust = inverse_thrust_model(a_z,battery_state.voltage,param,thrust_model); 59 | 60 | // X Translation Control 61 | e_x = ref.ref.pose.position.x - current_pose.pose.position.x; 62 | e_dx = ref.ref.twist.linear.x - current_twist.twist.linear.x; 63 | s_x = param.k_xe*e_x + e_dx; 64 | u_x = (param.k_xe*e_dx+ref.ref.accel.linear.x+param.k_xs*tanh(param.k_xt*s_x))/a_z; 65 | 66 | // Y Translation Control 67 | e_y = ref.ref.pose.position.y - current_pose.pose.position.y; 68 | e_dy = ref.ref.twist.linear.y - current_twist.twist.linear.y; 69 | s_y = param.k_ye*e_y + e_dy; 70 | u_y = (param.k_ye*e_dy+ref.ref.accel.linear.y+param.k_ys*tanh(param.k_yt*s_y))/a_z; 71 | 72 | // Calculate Target Eulers 73 | target_euler.x() = asin(u_x*sin(ref_euler.z()) - u_y*cos(ref_euler.z())); 74 | target_euler.y() = asin((u_x*cos(ref_euler.z()) + u_y*sin(ref_euler.z()))/cos(target_euler.x())); 75 | target_euler.z() = ref_euler.z(); 76 | attitude_target.orientation = rpy2q(target_euler); 77 | 78 | if (param.pub_debug){ 79 | pub_debug(); 80 | } 81 | 82 | return attitude_target; 83 | } 84 | 85 | double SLIDINGMODE::get_hover_thrust(){ 86 | return param.hover_thrust; 87 | } 88 | 89 | int SLIDINGMODE::sign(double& value){ 90 | if (value > 0){ 91 | return 1; 92 | } 93 | else if (value < 0){ 94 | return -1; 95 | } 96 | else{ 97 | return 0; 98 | } 99 | } -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_hess.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_e_hess_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | 38 | /* Symbol visibility in DLLs */ 39 | #ifndef CASADI_SYMBOL_EXPORT 40 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 41 | #if defined(STATIC_LINKED) 42 | #define CASADI_SYMBOL_EXPORT 43 | #else 44 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 45 | #endif 46 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 47 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 48 | #else 49 | #define CASADI_SYMBOL_EXPORT 50 | #endif 51 | #endif 52 | 53 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 54 | static const casadi_int casadi_s1[3] = {0, 0, 0}; 55 | static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2}; 56 | static const casadi_int casadi_s3[11] = {8, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 57 | 58 | /* quadrotor_cost_y_e_hess:(i0[8],i1[],i2[],i3[8],i4[3])->(o0[8x8,0nz]) */ 59 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 60 | return 0; 61 | } 62 | 63 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 64 | return casadi_f0(arg, res, iw, w, mem); 65 | } 66 | 67 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_hess_alloc_mem(void) { 68 | return 0; 69 | } 70 | 71 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_hess_init_mem(int mem) { 72 | return 0; 73 | } 74 | 75 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_hess_free_mem(int mem) { 76 | } 77 | 78 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_hess_checkout(void) { 79 | return 0; 80 | } 81 | 82 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_hess_release(int mem) { 83 | } 84 | 85 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_hess_incref(void) { 86 | } 87 | 88 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_hess_decref(void) { 89 | } 90 | 91 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_hess_n_in(void) { return 5;} 92 | 93 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_hess_n_out(void) { return 1;} 94 | 95 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_e_hess_default_in(casadi_int i) { 96 | switch (i) { 97 | default: return 0; 98 | } 99 | } 100 | 101 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_hess_name_in(casadi_int i) { 102 | switch (i) { 103 | case 0: return "i0"; 104 | case 1: return "i1"; 105 | case 2: return "i2"; 106 | case 3: return "i3"; 107 | case 4: return "i4"; 108 | default: return 0; 109 | } 110 | } 111 | 112 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_hess_name_out(casadi_int i) { 113 | switch (i) { 114 | case 0: return "o0"; 115 | default: return 0; 116 | } 117 | } 118 | 119 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_hess_sparsity_in(casadi_int i) { 120 | switch (i) { 121 | case 0: return casadi_s0; 122 | case 1: return casadi_s1; 123 | case 2: return casadi_s1; 124 | case 3: return casadi_s0; 125 | case 4: return casadi_s2; 126 | default: return 0; 127 | } 128 | } 129 | 130 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_hess_sparsity_out(casadi_int i) { 131 | switch (i) { 132 | case 0: return casadi_s3; 133 | default: return 0; 134 | } 135 | } 136 | 137 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 138 | if (sz_arg) *sz_arg = 5; 139 | if (sz_res) *sz_res = 1; 140 | if (sz_iw) *sz_iw = 0; 141 | if (sz_w) *sz_w = 0; 142 | return 0; 143 | } 144 | 145 | 146 | #ifdef __cplusplus 147 | } /* extern "C" */ 148 | #endif 149 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_hess.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_hess_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | #define casadi_s4 CASADI_PREFIX(s4) 38 | 39 | /* Symbol visibility in DLLs */ 40 | #ifndef CASADI_SYMBOL_EXPORT 41 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 42 | #if defined(STATIC_LINKED) 43 | #define CASADI_SYMBOL_EXPORT 44 | #else 45 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 46 | #endif 47 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 48 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 49 | #else 50 | #define CASADI_SYMBOL_EXPORT 51 | #endif 52 | #endif 53 | 54 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 55 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 56 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 57 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 58 | static const casadi_int casadi_s4[14] = {11, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 59 | 60 | /* quadrotor_cost_y_hess:(i0[8],i1[3],i2[],i3[11],i4[3])->(o0[11x11,0nz]) */ 61 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 62 | return 0; 63 | } 64 | 65 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 66 | return casadi_f0(arg, res, iw, w, mem); 67 | } 68 | 69 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_hess_alloc_mem(void) { 70 | return 0; 71 | } 72 | 73 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_hess_init_mem(int mem) { 74 | return 0; 75 | } 76 | 77 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_hess_free_mem(int mem) { 78 | } 79 | 80 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_hess_checkout(void) { 81 | return 0; 82 | } 83 | 84 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_hess_release(int mem) { 85 | } 86 | 87 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_hess_incref(void) { 88 | } 89 | 90 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_hess_decref(void) { 91 | } 92 | 93 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_hess_n_in(void) { return 5;} 94 | 95 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_hess_n_out(void) { return 1;} 96 | 97 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_hess_default_in(casadi_int i) { 98 | switch (i) { 99 | default: return 0; 100 | } 101 | } 102 | 103 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_hess_name_in(casadi_int i) { 104 | switch (i) { 105 | case 0: return "i0"; 106 | case 1: return "i1"; 107 | case 2: return "i2"; 108 | case 3: return "i3"; 109 | case 4: return "i4"; 110 | default: return 0; 111 | } 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_hess_name_out(casadi_int i) { 115 | switch (i) { 116 | case 0: return "o0"; 117 | default: return 0; 118 | } 119 | } 120 | 121 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_hess_sparsity_in(casadi_int i) { 122 | switch (i) { 123 | case 0: return casadi_s0; 124 | case 1: return casadi_s1; 125 | case 2: return casadi_s2; 126 | case 3: return casadi_s3; 127 | case 4: return casadi_s1; 128 | default: return 0; 129 | } 130 | } 131 | 132 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_hess_sparsity_out(casadi_int i) { 133 | switch (i) { 134 | case 0: return casadi_s4; 135 | default: return 0; 136 | } 137 | } 138 | 139 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 140 | if (sz_arg) *sz_arg = 5; 141 | if (sz_res) *sz_res = 1; 142 | if (sz_iw) *sz_iw = 0; 143 | if (sz_w) *sz_w = 0; 144 | return 0; 145 | } 146 | 147 | 148 | #ifdef __cplusplus 149 | } /* extern "C" */ 150 | #endif 151 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_hess.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_0_hess_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | #define casadi_s4 CASADI_PREFIX(s4) 38 | 39 | /* Symbol visibility in DLLs */ 40 | #ifndef CASADI_SYMBOL_EXPORT 41 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 42 | #if defined(STATIC_LINKED) 43 | #define CASADI_SYMBOL_EXPORT 44 | #else 45 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 46 | #endif 47 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 48 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 49 | #else 50 | #define CASADI_SYMBOL_EXPORT 51 | #endif 52 | #endif 53 | 54 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 55 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 56 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 57 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 58 | static const casadi_int casadi_s4[14] = {11, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 59 | 60 | /* quadrotor_cost_y_0_hess:(i0[8],i1[3],i2[],i3[11],i4[3])->(o0[11x11,0nz]) */ 61 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 62 | return 0; 63 | } 64 | 65 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 66 | return casadi_f0(arg, res, iw, w, mem); 67 | } 68 | 69 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_hess_alloc_mem(void) { 70 | return 0; 71 | } 72 | 73 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_hess_init_mem(int mem) { 74 | return 0; 75 | } 76 | 77 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_hess_free_mem(int mem) { 78 | } 79 | 80 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_hess_checkout(void) { 81 | return 0; 82 | } 83 | 84 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_hess_release(int mem) { 85 | } 86 | 87 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_hess_incref(void) { 88 | } 89 | 90 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_hess_decref(void) { 91 | } 92 | 93 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_hess_n_in(void) { return 5;} 94 | 95 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_hess_n_out(void) { return 1;} 96 | 97 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_0_hess_default_in(casadi_int i) { 98 | switch (i) { 99 | default: return 0; 100 | } 101 | } 102 | 103 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_hess_name_in(casadi_int i) { 104 | switch (i) { 105 | case 0: return "i0"; 106 | case 1: return "i1"; 107 | case 2: return "i2"; 108 | case 3: return "i3"; 109 | case 4: return "i4"; 110 | default: return 0; 111 | } 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_hess_name_out(casadi_int i) { 115 | switch (i) { 116 | case 0: return "o0"; 117 | default: return 0; 118 | } 119 | } 120 | 121 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_hess_sparsity_in(casadi_int i) { 122 | switch (i) { 123 | case 0: return casadi_s0; 124 | case 1: return casadi_s1; 125 | case 2: return casadi_s2; 126 | case 3: return casadi_s3; 127 | case 4: return casadi_s1; 128 | default: return 0; 129 | } 130 | } 131 | 132 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_hess_sparsity_out(casadi_int i) { 133 | switch (i) { 134 | case 0: return casadi_s4; 135 | default: return 0; 136 | } 137 | } 138 | 139 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 140 | if (sz_arg) *sz_arg = 5; 141 | if (sz_res) *sz_res = 1; 142 | if (sz_iw) *sz_iw = 0; 143 | if (sz_w) *sz_w = 0; 144 | return 0; 145 | } 146 | 147 | 148 | #ifdef __cplusplus 149 | } /* extern "C" */ 150 | #endif 151 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_e_fun_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | 37 | /* Symbol visibility in DLLs */ 38 | #ifndef CASADI_SYMBOL_EXPORT 39 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 40 | #if defined(STATIC_LINKED) 41 | #define CASADI_SYMBOL_EXPORT 42 | #else 43 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 44 | #endif 45 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 46 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 47 | #else 48 | #define CASADI_SYMBOL_EXPORT 49 | #endif 50 | #endif 51 | 52 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 53 | static const casadi_int casadi_s1[3] = {0, 0, 0}; 54 | static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2}; 55 | 56 | /* quadrotor_cost_y_e_fun:(i0[8],i1[],i2[],i3[3])->(o0[8]) */ 57 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 58 | casadi_real a0; 59 | a0=arg[0]? arg[0][0] : 0; 60 | if (res[0]!=0) res[0][0]=a0; 61 | a0=arg[0]? arg[0][1] : 0; 62 | if (res[0]!=0) res[0][1]=a0; 63 | a0=arg[0]? arg[0][2] : 0; 64 | if (res[0]!=0) res[0][2]=a0; 65 | a0=arg[0]? arg[0][3] : 0; 66 | if (res[0]!=0) res[0][3]=a0; 67 | a0=arg[0]? arg[0][4] : 0; 68 | if (res[0]!=0) res[0][4]=a0; 69 | a0=arg[0]? arg[0][5] : 0; 70 | if (res[0]!=0) res[0][5]=a0; 71 | a0=arg[0]? arg[0][6] : 0; 72 | if (res[0]!=0) res[0][6]=a0; 73 | a0=arg[0]? arg[0][7] : 0; 74 | if (res[0]!=0) res[0][7]=a0; 75 | return 0; 76 | } 77 | 78 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 79 | return casadi_f0(arg, res, iw, w, mem); 80 | } 81 | 82 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_alloc_mem(void) { 83 | return 0; 84 | } 85 | 86 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_init_mem(int mem) { 87 | return 0; 88 | } 89 | 90 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_free_mem(int mem) { 91 | } 92 | 93 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_checkout(void) { 94 | return 0; 95 | } 96 | 97 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_release(int mem) { 98 | } 99 | 100 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_incref(void) { 101 | } 102 | 103 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_decref(void) { 104 | } 105 | 106 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_fun_n_in(void) { return 4;} 107 | 108 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_fun_n_out(void) { return 1;} 109 | 110 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_e_fun_default_in(casadi_int i) { 111 | switch (i) { 112 | default: return 0; 113 | } 114 | } 115 | 116 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_fun_name_in(casadi_int i) { 117 | switch (i) { 118 | case 0: return "i0"; 119 | case 1: return "i1"; 120 | case 2: return "i2"; 121 | case 3: return "i3"; 122 | default: return 0; 123 | } 124 | } 125 | 126 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_fun_name_out(casadi_int i) { 127 | switch (i) { 128 | case 0: return "o0"; 129 | default: return 0; 130 | } 131 | } 132 | 133 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_fun_sparsity_in(casadi_int i) { 134 | switch (i) { 135 | case 0: return casadi_s0; 136 | case 1: return casadi_s1; 137 | case 2: return casadi_s1; 138 | case 3: return casadi_s2; 139 | default: return 0; 140 | } 141 | } 142 | 143 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_fun_sparsity_out(casadi_int i) { 144 | switch (i) { 145 | case 0: return casadi_s0; 146 | default: return 0; 147 | } 148 | } 149 | 150 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 151 | if (sz_arg) *sz_arg = 4; 152 | if (sz_res) *sz_res = 1; 153 | if (sz_iw) *sz_iw = 0; 154 | if (sz_w) *sz_w = 0; 155 | return 0; 156 | } 157 | 158 | 159 | #ifdef __cplusplus 160 | } /* extern "C" */ 161 | #endif 162 | -------------------------------------------------------------------------------- /airo_control/src/example_mission_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | geometry_msgs::PoseStamped local_pose; 11 | airo_message::FSMInfo fsm_info; 12 | 13 | enum State{ 14 | TAKEOFF, 15 | COMMAND, 16 | LAND 17 | }; 18 | 19 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ 20 | local_pose.header = msg->header; 21 | local_pose.pose = msg->pose; 22 | } 23 | 24 | void fsm_info_cb(const airo_message::FSMInfo::ConstPtr& msg){ 25 | fsm_info.header = msg->header; 26 | fsm_info.is_landed = msg->is_landed; 27 | fsm_info.is_waiting_for_command = msg->is_waiting_for_command; 28 | } 29 | 30 | int main(int argc, char **argv) 31 | { 32 | ros::init(argc, argv, "offb_node"); 33 | ros::NodeHandle nh; 34 | ros::Rate rate(20.0); 35 | State state = TAKEOFF; 36 | 37 | airo_message::ReferenceStamped target_pose_1; 38 | airo_message::ReferenceStamped target_pose_2; 39 | airo_message::TakeoffLandTrigger takeoff_land_trigger; 40 | bool target_1_reached = false; 41 | 42 | ros::Subscriber local_pose_sub = nh.subscribe("/mavros/local_position/pose",100,pose_cb); 43 | ros::Subscriber fsm_info_sub = nh.subscribe("/airo_control/fsm_info",10,fsm_info_cb); 44 | ros::Publisher command_pub = nh.advertise("/airo_control/setpoint",10); 45 | ros::Publisher takeoff_land_pub = nh.advertise("/airo_control/takeoff_land_trigger",10); 46 | 47 | target_pose_1.ref.pose.position.x = 0; 48 | target_pose_1.ref.pose.position.y = -2.5; 49 | target_pose_1.ref.pose.position.z = 1.5; 50 | target_pose_1.ref.pose.orientation.w = 0.9238; 51 | target_pose_1.ref.pose.orientation.x = 0.0; 52 | target_pose_1.ref.pose.orientation.y = 0.0; 53 | target_pose_1.ref.pose.orientation.z = 0.3826; 54 | 55 | target_pose_2.ref.pose.position.x = 0; 56 | target_pose_2.ref.pose.position.y = 2.5; 57 | target_pose_2.ref.pose.position.z = 1.5; 58 | target_pose_2.ref.pose.orientation.w = 1.0; 59 | target_pose_2.ref.pose.orientation.x = 0.0; 60 | target_pose_2.ref.pose.orientation.y = 0.0; 61 | target_pose_2.ref.pose.orientation.z = 0.0; 62 | 63 | while(ros::ok()){ 64 | switch(state){ 65 | case TAKEOFF:{ 66 | if(fsm_info.is_landed == true){ 67 | while(ros::ok()){ 68 | takeoff_land_trigger.takeoff_land_trigger = true; // Takeoff 69 | takeoff_land_trigger.header.stamp = ros::Time::now(); 70 | takeoff_land_pub.publish(takeoff_land_trigger); 71 | ros::spinOnce(); 72 | ros::Duration(0.5).sleep(); 73 | if(fsm_info.is_waiting_for_command){ 74 | state = COMMAND; 75 | break; 76 | } 77 | } 78 | } 79 | break; 80 | } 81 | 82 | case COMMAND:{ 83 | if(fsm_info.is_waiting_for_command){ 84 | if(!target_1_reached){ 85 | target_pose_1.header.stamp = ros::Time::now(); 86 | command_pub.publish(target_pose_1); 87 | if(abs(local_pose.pose.position.x - target_pose_1.ref.pose.position.x) 88 | + abs(local_pose.pose.position.y - target_pose_1.ref.pose.position.y) 89 | + abs(local_pose.pose.position.z - target_pose_1.ref.pose.position.z) < 0.5){ 90 | target_1_reached = true; 91 | } 92 | } 93 | else{ 94 | target_pose_2.header.stamp = ros::Time::now(); 95 | command_pub.publish(target_pose_2); 96 | if(abs(local_pose.pose.position.x - target_pose_2.ref.pose.position.x) 97 | + abs(local_pose.pose.position.y - target_pose_2.ref.pose.position.y) 98 | + abs(local_pose.pose.position.z - target_pose_2.ref.pose.position.z) < 0.5){ 99 | state = LAND; 100 | } 101 | } 102 | } 103 | break; 104 | } 105 | 106 | case LAND:{ 107 | if(fsm_info.is_waiting_for_command){ 108 | takeoff_land_trigger.takeoff_land_trigger = false; // Land 109 | takeoff_land_trigger.header.stamp = ros::Time::now(); 110 | takeoff_land_pub.publish(takeoff_land_trigger); 111 | } 112 | break; 113 | } 114 | } 115 | 116 | ros::spinOnce(); 117 | ros::Duration(rate).sleep(); 118 | } 119 | 120 | return 0; 121 | } -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_fun_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | 38 | /* Symbol visibility in DLLs */ 39 | #ifndef CASADI_SYMBOL_EXPORT 40 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 41 | #if defined(STATIC_LINKED) 42 | #define CASADI_SYMBOL_EXPORT 43 | #else 44 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 45 | #endif 46 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 47 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 48 | #else 49 | #define CASADI_SYMBOL_EXPORT 50 | #endif 51 | #endif 52 | 53 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 54 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 55 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 56 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 57 | 58 | /* quadrotor_cost_y_fun:(i0[8],i1[3],i2[],i3[3])->(o0[11]) */ 59 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 60 | casadi_real a0; 61 | a0=arg[0]? arg[0][0] : 0; 62 | if (res[0]!=0) res[0][0]=a0; 63 | a0=arg[0]? arg[0][1] : 0; 64 | if (res[0]!=0) res[0][1]=a0; 65 | a0=arg[0]? arg[0][2] : 0; 66 | if (res[0]!=0) res[0][2]=a0; 67 | a0=arg[0]? arg[0][3] : 0; 68 | if (res[0]!=0) res[0][3]=a0; 69 | a0=arg[0]? arg[0][4] : 0; 70 | if (res[0]!=0) res[0][4]=a0; 71 | a0=arg[0]? arg[0][5] : 0; 72 | if (res[0]!=0) res[0][5]=a0; 73 | a0=arg[0]? arg[0][6] : 0; 74 | if (res[0]!=0) res[0][6]=a0; 75 | a0=arg[0]? arg[0][7] : 0; 76 | if (res[0]!=0) res[0][7]=a0; 77 | a0=arg[1]? arg[1][0] : 0; 78 | if (res[0]!=0) res[0][8]=a0; 79 | a0=arg[1]? arg[1][1] : 0; 80 | if (res[0]!=0) res[0][9]=a0; 81 | a0=arg[1]? arg[1][2] : 0; 82 | if (res[0]!=0) res[0][10]=a0; 83 | return 0; 84 | } 85 | 86 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 87 | return casadi_f0(arg, res, iw, w, mem); 88 | } 89 | 90 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_alloc_mem(void) { 91 | return 0; 92 | } 93 | 94 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_init_mem(int mem) { 95 | return 0; 96 | } 97 | 98 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_free_mem(int mem) { 99 | } 100 | 101 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_checkout(void) { 102 | return 0; 103 | } 104 | 105 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_release(int mem) { 106 | } 107 | 108 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_incref(void) { 109 | } 110 | 111 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_decref(void) { 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_fun_n_in(void) { return 4;} 115 | 116 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_fun_n_out(void) { return 1;} 117 | 118 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_fun_default_in(casadi_int i) { 119 | switch (i) { 120 | default: return 0; 121 | } 122 | } 123 | 124 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_fun_name_in(casadi_int i) { 125 | switch (i) { 126 | case 0: return "i0"; 127 | case 1: return "i1"; 128 | case 2: return "i2"; 129 | case 3: return "i3"; 130 | default: return 0; 131 | } 132 | } 133 | 134 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_fun_name_out(casadi_int i) { 135 | switch (i) { 136 | case 0: return "o0"; 137 | default: return 0; 138 | } 139 | } 140 | 141 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_fun_sparsity_in(casadi_int i) { 142 | switch (i) { 143 | case 0: return casadi_s0; 144 | case 1: return casadi_s1; 145 | case 2: return casadi_s2; 146 | case 3: return casadi_s1; 147 | default: return 0; 148 | } 149 | } 150 | 151 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_fun_sparsity_out(casadi_int i) { 152 | switch (i) { 153 | case 0: return casadi_s3; 154 | default: return 0; 155 | } 156 | } 157 | 158 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 159 | if (sz_arg) *sz_arg = 4; 160 | if (sz_res) *sz_res = 1; 161 | if (sz_iw) *sz_iw = 0; 162 | if (sz_w) *sz_w = 0; 163 | return 0; 164 | } 165 | 166 | 167 | #ifdef __cplusplus 168 | } /* extern "C" */ 169 | #endif 170 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_0_fun_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | 38 | /* Symbol visibility in DLLs */ 39 | #ifndef CASADI_SYMBOL_EXPORT 40 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 41 | #if defined(STATIC_LINKED) 42 | #define CASADI_SYMBOL_EXPORT 43 | #else 44 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 45 | #endif 46 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 47 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 48 | #else 49 | #define CASADI_SYMBOL_EXPORT 50 | #endif 51 | #endif 52 | 53 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 54 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 55 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 56 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 57 | 58 | /* quadrotor_cost_y_0_fun:(i0[8],i1[3],i2[],i3[3])->(o0[11]) */ 59 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 60 | casadi_real a0; 61 | a0=arg[0]? arg[0][0] : 0; 62 | if (res[0]!=0) res[0][0]=a0; 63 | a0=arg[0]? arg[0][1] : 0; 64 | if (res[0]!=0) res[0][1]=a0; 65 | a0=arg[0]? arg[0][2] : 0; 66 | if (res[0]!=0) res[0][2]=a0; 67 | a0=arg[0]? arg[0][3] : 0; 68 | if (res[0]!=0) res[0][3]=a0; 69 | a0=arg[0]? arg[0][4] : 0; 70 | if (res[0]!=0) res[0][4]=a0; 71 | a0=arg[0]? arg[0][5] : 0; 72 | if (res[0]!=0) res[0][5]=a0; 73 | a0=arg[0]? arg[0][6] : 0; 74 | if (res[0]!=0) res[0][6]=a0; 75 | a0=arg[0]? arg[0][7] : 0; 76 | if (res[0]!=0) res[0][7]=a0; 77 | a0=arg[1]? arg[1][0] : 0; 78 | if (res[0]!=0) res[0][8]=a0; 79 | a0=arg[1]? arg[1][1] : 0; 80 | if (res[0]!=0) res[0][9]=a0; 81 | a0=arg[1]? arg[1][2] : 0; 82 | if (res[0]!=0) res[0][10]=a0; 83 | return 0; 84 | } 85 | 86 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 87 | return casadi_f0(arg, res, iw, w, mem); 88 | } 89 | 90 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_alloc_mem(void) { 91 | return 0; 92 | } 93 | 94 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_init_mem(int mem) { 95 | return 0; 96 | } 97 | 98 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_free_mem(int mem) { 99 | } 100 | 101 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_checkout(void) { 102 | return 0; 103 | } 104 | 105 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_release(int mem) { 106 | } 107 | 108 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_incref(void) { 109 | } 110 | 111 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_decref(void) { 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_fun_n_in(void) { return 4;} 115 | 116 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_fun_n_out(void) { return 1;} 117 | 118 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_0_fun_default_in(casadi_int i) { 119 | switch (i) { 120 | default: return 0; 121 | } 122 | } 123 | 124 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_fun_name_in(casadi_int i) { 125 | switch (i) { 126 | case 0: return "i0"; 127 | case 1: return "i1"; 128 | case 2: return "i2"; 129 | case 3: return "i3"; 130 | default: return 0; 131 | } 132 | } 133 | 134 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_fun_name_out(casadi_int i) { 135 | switch (i) { 136 | case 0: return "o0"; 137 | default: return 0; 138 | } 139 | } 140 | 141 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_fun_sparsity_in(casadi_int i) { 142 | switch (i) { 143 | case 0: return casadi_s0; 144 | case 1: return casadi_s1; 145 | case 2: return casadi_s2; 146 | case 3: return casadi_s1; 147 | default: return 0; 148 | } 149 | } 150 | 151 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_fun_sparsity_out(casadi_int i) { 152 | switch (i) { 153 | case 0: return casadi_s3; 154 | default: return 0; 155 | } 156 | } 157 | 158 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 159 | if (sz_arg) *sz_arg = 4; 160 | if (sz_res) *sz_res = 1; 161 | if (sz_iw) *sz_iw = 0; 162 | if (sz_w) *sz_w = 0; 163 | return 0; 164 | } 165 | 166 | 167 | #ifdef __cplusplus 168 | } /* extern "C" */ 169 | #endif 170 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, 3 | * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, 4 | * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, 5 | * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl 6 | * 7 | * This file is part of acados. 8 | * 9 | * The 2-Clause BSD License 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE.; 32 | */ 33 | 34 | 35 | #ifndef quadrotor_COST 36 | #define quadrotor_COST 37 | 38 | #ifdef __cplusplus 39 | extern "C" { 40 | #endif 41 | 42 | 43 | // Cost at initial shooting node 44 | 45 | int quadrotor_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 46 | int quadrotor_cost_y_0_fun_work(int *, int *, int *, int *); 47 | const int *quadrotor_cost_y_0_fun_sparsity_in(int); 48 | const int *quadrotor_cost_y_0_fun_sparsity_out(int); 49 | int quadrotor_cost_y_0_fun_n_in(void); 50 | int quadrotor_cost_y_0_fun_n_out(void); 51 | 52 | int quadrotor_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 53 | int quadrotor_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); 54 | const int *quadrotor_cost_y_0_fun_jac_ut_xt_sparsity_in(int); 55 | const int *quadrotor_cost_y_0_fun_jac_ut_xt_sparsity_out(int); 56 | int quadrotor_cost_y_0_fun_jac_ut_xt_n_in(void); 57 | int quadrotor_cost_y_0_fun_jac_ut_xt_n_out(void); 58 | 59 | int quadrotor_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 60 | int quadrotor_cost_y_0_hess_work(int *, int *, int *, int *); 61 | const int *quadrotor_cost_y_0_hess_sparsity_in(int); 62 | const int *quadrotor_cost_y_0_hess_sparsity_out(int); 63 | int quadrotor_cost_y_0_hess_n_in(void); 64 | int quadrotor_cost_y_0_hess_n_out(void); 65 | 66 | 67 | 68 | // Cost at path shooting node 69 | 70 | int quadrotor_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 71 | int quadrotor_cost_y_fun_work(int *, int *, int *, int *); 72 | const int *quadrotor_cost_y_fun_sparsity_in(int); 73 | const int *quadrotor_cost_y_fun_sparsity_out(int); 74 | int quadrotor_cost_y_fun_n_in(void); 75 | int quadrotor_cost_y_fun_n_out(void); 76 | 77 | int quadrotor_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 78 | int quadrotor_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); 79 | const int *quadrotor_cost_y_fun_jac_ut_xt_sparsity_in(int); 80 | const int *quadrotor_cost_y_fun_jac_ut_xt_sparsity_out(int); 81 | int quadrotor_cost_y_fun_jac_ut_xt_n_in(void); 82 | int quadrotor_cost_y_fun_jac_ut_xt_n_out(void); 83 | 84 | int quadrotor_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 85 | int quadrotor_cost_y_hess_work(int *, int *, int *, int *); 86 | const int *quadrotor_cost_y_hess_sparsity_in(int); 87 | const int *quadrotor_cost_y_hess_sparsity_out(int); 88 | int quadrotor_cost_y_hess_n_in(void); 89 | int quadrotor_cost_y_hess_n_out(void); 90 | 91 | 92 | 93 | // Cost at terminal shooting node 94 | 95 | int quadrotor_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 96 | int quadrotor_cost_y_e_fun_work(int *, int *, int *, int *); 97 | const int *quadrotor_cost_y_e_fun_sparsity_in(int); 98 | const int *quadrotor_cost_y_e_fun_sparsity_out(int); 99 | int quadrotor_cost_y_e_fun_n_in(void); 100 | int quadrotor_cost_y_e_fun_n_out(void); 101 | 102 | int quadrotor_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 103 | int quadrotor_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); 104 | const int *quadrotor_cost_y_e_fun_jac_ut_xt_sparsity_in(int); 105 | const int *quadrotor_cost_y_e_fun_jac_ut_xt_sparsity_out(int); 106 | int quadrotor_cost_y_e_fun_jac_ut_xt_n_in(void); 107 | int quadrotor_cost_y_e_fun_jac_ut_xt_n_out(void); 108 | 109 | int quadrotor_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); 110 | int quadrotor_cost_y_e_hess_work(int *, int *, int *, int *); 111 | const int *quadrotor_cost_y_e_hess_sparsity_in(int); 112 | const int *quadrotor_cost_y_e_hess_sparsity_out(int); 113 | int quadrotor_cost_y_e_hess_n_in(void); 114 | int quadrotor_cost_y_e_hess_n_out(void); 115 | 116 | 117 | 118 | #ifdef __cplusplus 119 | } /* extern "C" */ 120 | #endif 121 | 122 | #endif // quadrotor_COST 123 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_ode_fun.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_expl_ode_fun_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | 36 | /* Symbol visibility in DLLs */ 37 | #ifndef CASADI_SYMBOL_EXPORT 38 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 39 | #if defined(STATIC_LINKED) 40 | #define CASADI_SYMBOL_EXPORT 41 | #else 42 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 43 | #endif 44 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 45 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 46 | #else 47 | #define CASADI_SYMBOL_EXPORT 48 | #endif 49 | #endif 50 | 51 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 52 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 53 | 54 | /* quadrotor_expl_ode_fun:(i0[8],i1[3],i2[3])->(o0[8]) */ 55 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 56 | casadi_real a0, a1, a2, a3, a4, a5; 57 | a0=arg[0]? arg[0][3] : 0; 58 | if (res[0]!=0) res[0][0]=a0; 59 | a0=arg[0]? arg[0][4] : 0; 60 | if (res[0]!=0) res[0][1]=a0; 61 | a0=arg[0]? arg[0][5] : 0; 62 | if (res[0]!=0) res[0][2]=a0; 63 | a0=arg[0]? arg[0][6] : 0; 64 | a1=cos(a0); 65 | a2=arg[0]? arg[0][7] : 0; 66 | a3=sin(a2); 67 | a1=(a1*a3); 68 | a3=arg[2]? arg[2][2] : 0; 69 | a4=cos(a3); 70 | a1=(a1*a4); 71 | a4=sin(a0); 72 | a5=sin(a3); 73 | a4=(a4*a5); 74 | a1=(a1+a4); 75 | a4=arg[1]? arg[1][0] : 0; 76 | a1=(a1*a4); 77 | if (res[0]!=0) res[0][3]=a1; 78 | a1=cos(a0); 79 | a5=sin(a2); 80 | a1=(a1*a5); 81 | a5=sin(a3); 82 | a1=(a1*a5); 83 | a5=sin(a0); 84 | a3=cos(a3); 85 | a5=(a5*a3); 86 | a1=(a1-a5); 87 | a1=(a1*a4); 88 | if (res[0]!=0) res[0][4]=a1; 89 | a1=-9.8066499999999994e+00; 90 | a5=cos(a2); 91 | a3=cos(a0); 92 | a5=(a5*a3); 93 | a5=(a5*a4); 94 | a1=(a1+a5); 95 | if (res[0]!=0) res[0][5]=a1; 96 | a1=arg[1]? arg[1][1] : 0; 97 | a1=(a1-a0); 98 | a0=arg[2]? arg[2][0] : 0; 99 | a1=(a1/a0); 100 | if (res[0]!=0) res[0][6]=a1; 101 | a1=arg[1]? arg[1][2] : 0; 102 | a1=(a1-a2); 103 | a2=arg[2]? arg[2][1] : 0; 104 | a1=(a1/a2); 105 | if (res[0]!=0) res[0][7]=a1; 106 | return 0; 107 | } 108 | 109 | CASADI_SYMBOL_EXPORT int quadrotor_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 110 | return casadi_f0(arg, res, iw, w, mem); 111 | } 112 | 113 | CASADI_SYMBOL_EXPORT int quadrotor_expl_ode_fun_alloc_mem(void) { 114 | return 0; 115 | } 116 | 117 | CASADI_SYMBOL_EXPORT int quadrotor_expl_ode_fun_init_mem(int mem) { 118 | return 0; 119 | } 120 | 121 | CASADI_SYMBOL_EXPORT void quadrotor_expl_ode_fun_free_mem(int mem) { 122 | } 123 | 124 | CASADI_SYMBOL_EXPORT int quadrotor_expl_ode_fun_checkout(void) { 125 | return 0; 126 | } 127 | 128 | CASADI_SYMBOL_EXPORT void quadrotor_expl_ode_fun_release(int mem) { 129 | } 130 | 131 | CASADI_SYMBOL_EXPORT void quadrotor_expl_ode_fun_incref(void) { 132 | } 133 | 134 | CASADI_SYMBOL_EXPORT void quadrotor_expl_ode_fun_decref(void) { 135 | } 136 | 137 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_ode_fun_n_in(void) { return 3;} 138 | 139 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_ode_fun_n_out(void) { return 1;} 140 | 141 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_expl_ode_fun_default_in(casadi_int i) { 142 | switch (i) { 143 | default: return 0; 144 | } 145 | } 146 | 147 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_ode_fun_name_in(casadi_int i) { 148 | switch (i) { 149 | case 0: return "i0"; 150 | case 1: return "i1"; 151 | case 2: return "i2"; 152 | default: return 0; 153 | } 154 | } 155 | 156 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_ode_fun_name_out(casadi_int i) { 157 | switch (i) { 158 | case 0: return "o0"; 159 | default: return 0; 160 | } 161 | } 162 | 163 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_ode_fun_sparsity_in(casadi_int i) { 164 | switch (i) { 165 | case 0: return casadi_s0; 166 | case 1: return casadi_s1; 167 | case 2: return casadi_s1; 168 | default: return 0; 169 | } 170 | } 171 | 172 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_ode_fun_sparsity_out(casadi_int i) { 173 | switch (i) { 174 | case 0: return casadi_s0; 175 | default: return 0; 176 | } 177 | } 178 | 179 | CASADI_SYMBOL_EXPORT int quadrotor_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 180 | if (sz_arg) *sz_arg = 3; 181 | if (sz_res) *sz_res = 1; 182 | if (sz_iw) *sz_iw = 0; 183 | if (sz_w) *sz_w = 0; 184 | return 0; 185 | } 186 | 187 | 188 | #ifdef __cplusplus 189 | } /* extern "C" */ 190 | #endif 191 | -------------------------------------------------------------------------------- /airo_control/include/airo_control/airo_control_fsm.h: -------------------------------------------------------------------------------- 1 | #ifndef AIRO_CONTROL_FSM_H 2 | #define AIRO_CONTROL_FSM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 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 | #include "airo_control/rc_input.h" 25 | #include "airo_control/controller/mpc.h" 26 | #include "airo_control/controller/backstepping.h" 27 | #include "airo_control/controller/slidingmode.h" 28 | 29 | class AIRO_CONTROL_FSM{ 30 | private: 31 | 32 | enum STATE_FSM{ 33 | RC_MANUAL, 34 | AUTO_HOVER, 35 | AUTO_TAKEOFF, 36 | AUTO_LAND, 37 | POS_COMMAND 38 | }; 39 | 40 | // Parameters 41 | std::string CONTROLLER_TYPE; 42 | std::string POSE_TOPIC; 43 | std::string TWIST_TOPIC; 44 | double STATE_TIMEOUT,RC_TIMEOUT,ODOM_TIMEOUT,COMMAND_TIMEOUT; 45 | double MOTOR_SPEEDUP_TIME; 46 | double TAKEOFF_HEIGHT; 47 | double TAKEOFF_LAND_SPEED; 48 | double REJECT_TAKEOFF_TWIST_THRESHOLD; 49 | double HOVER_MAX_VELOCITY; 50 | double HOVER_MAX_YAW_RATE; 51 | bool CHECK_SAFETY_VOLUMN; 52 | std::vector SAFETY_VOLUMN; // min_x max_x min_y max_y max_z 53 | bool WITHOUT_RC; 54 | bool ENABLE_LOGGING; 55 | 56 | // Variables 57 | STATE_FSM state_fsm; 58 | RC_INPUT rc_input; 59 | RC_INPUT::RC_PARAM rc_param; 60 | bool solve_controller; 61 | bool is_landed; 62 | bool is_armed; 63 | bool enable_preview = false; // Only for MPC 64 | bool use_preview = false; // Only for MPC 65 | 66 | // Times 67 | ros::Time current_time; 68 | ros::Time takeoff_land_time; 69 | ros::Time last_hover_time; 70 | 71 | // ROS Sub & Pub 72 | ros::Subscriber pose_sub; 73 | ros::Subscriber twist_sub; 74 | ros::Subscriber imu_sub; 75 | ros::Subscriber state_sub; 76 | ros::Subscriber extended_state_sub; 77 | ros::Subscriber rc_input_sub; 78 | ros::Subscriber command_sub; 79 | ros::Subscriber command_preview_sub; 80 | ros::Subscriber takeoff_land_sub; 81 | ros::Subscriber battery_state_sub; 82 | ros::Publisher setpoint_pub; 83 | ros::Publisher fsm_info_pub; 84 | 85 | // ROS Services 86 | ros::ServiceClient setmode_srv; 87 | ros::ServiceClient arm_srv; 88 | ros::ServiceClient reboot_srv; 89 | 90 | // Messages 91 | airo_message::TakeoffLandTrigger takeoff_land_trigger; // 1 for takeoff 0 for landing 92 | airo_message::FSMInfo fsm_info; 93 | airo_message::ReferenceStamped controller_ref; 94 | airo_message::ReferenceStamped external_command; 95 | airo_message::ReferencePreview controller_ref_preview; 96 | airo_message::ReferencePreview external_command_preview; 97 | geometry_msgs::PoseStamped local_pose; 98 | geometry_msgs::PoseStamped takeoff_land_pose; 99 | geometry_msgs::TwistStamped local_twist; 100 | geometry_msgs::AccelStamped local_accel; 101 | mavros_msgs::AttitudeTarget attitude_target; 102 | mavros_msgs::State current_state; 103 | mavros_msgs::State previous_state; 104 | mavros_msgs::ExtendedState current_extended_state; 105 | sensor_msgs::BatteryState battery_state; 106 | 107 | // Controller 108 | std::unique_ptr controller; 109 | 110 | // For data logging 111 | std::string log_path; 112 | std::vector line_to_push; 113 | bool log_started = false; 114 | ros::Time log_start_time; 115 | 116 | public: 117 | 118 | AIRO_CONTROL_FSM(ros::NodeHandle&); 119 | void process(); 120 | void fsm(); 121 | bool check_connection(const ros::Time&); 122 | void publish_control_commands(mavros_msgs::AttitudeTarget,ros::Time); 123 | bool toggle_offboard(bool); 124 | bool toggle_arm(bool); 125 | void get_motor_speedup(); 126 | void set_ref(const geometry_msgs::PoseStamped&); 127 | void set_takeoff_land_ref(const double); 128 | void set_ref_with_rc(); 129 | void set_ref_with_external_command(); 130 | void set_ref_with_external_command_preview(); 131 | double extract_yaw_from_quaternion(const geometry_msgs::Quaternion&); 132 | void land_detector(); 133 | void motor_idle_and_disarm(); 134 | void takeoff_land_init(); 135 | void auto_hover_init(); 136 | geometry_msgs::Point check_safety_volumn(const geometry_msgs::Point&); 137 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr&); 138 | void twist_cb(const geometry_msgs::TwistStamped::ConstPtr&); 139 | void imu_cb(const sensor_msgs::Imu::ConstPtr&); 140 | void state_cb(const mavros_msgs::State::ConstPtr&); 141 | void extended_state_cb(const mavros_msgs::ExtendedState::ConstPtr&); 142 | void rc_input_cb(const mavros_msgs::RCIn::ConstPtr&); 143 | void external_command_cb(const airo_message::ReferenceStamped::ConstPtr&); 144 | void external_command_preview_cb(const airo_message::ReferencePreview::ConstPtr&); 145 | void takeoff_land_cb(const airo_message::TakeoffLandTrigger::ConstPtr&); 146 | void battery_state_cb(const sensor_msgs::BatteryState::ConstPtr&); 147 | bool state_received(const ros::Time&); 148 | bool rc_received(const ros::Time&); 149 | bool odom_received(const ros::Time&); 150 | bool external_command_received(const ros::Time&); 151 | bool external_command_preview_received(const ros::Time&); 152 | bool takeoff_land_received(const ros::Time&); 153 | bool battery_status_received(const ros::Time&); 154 | bool takeoff_trigered(const ros::Time&); 155 | bool land_trigered(const ros::Time&); 156 | double twist_norm(const geometry_msgs::TwistStamped); 157 | void reboot(); 158 | void init_log(); 159 | void update_log(); 160 | }; 161 | 162 | #endif -------------------------------------------------------------------------------- /airo_control/acados_scripts/quadrotor_model.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel 2 | from casadi import SX, vertcat, sin, cos 3 | import numpy as np 4 | import math 5 | from scipy.linalg import block_diag 6 | 7 | def export_quadrotor_model() -> AcadosModel: 8 | 9 | model_name = 'quadrotor' 10 | 11 | # constent parameters 12 | g = 9.80665 # gravity constant [m/s^2] 13 | 14 | # states 15 | x = SX.sym('x') # earth position x 16 | y = SX.sym('y') # earth position y 17 | z = SX.sym('z') # earth position z 18 | u = SX.sym('u') # earth velocity x 19 | v = SX.sym('v') # earth velocity y 20 | w = SX.sym('w') # earth velocity z 21 | phi = SX.sym('phi') # roll angle phi 22 | theta = SX.sym('theta') # pitch angle 23 | sym_x = vertcat(x,y,z,u,v,w,phi,theta) 24 | 25 | # controls 26 | a_z = SX.sym('a_z') # thrust command 27 | phi_cmd = SX.sym('phi_cmd') # roll angle command 28 | theta_cmd = SX.sym('theta_cmd') # pitch angle command 29 | sym_u = vertcat(a_z,phi_cmd,theta_cmd) 30 | 31 | # parameters 32 | tau_phi = SX.sym('tau_phi') 33 | tau_theta = SX.sym('tau_theta') 34 | psi = SX.sym('psi') # yaw angle 35 | sym_p = vertcat(tau_phi,tau_theta,psi) 36 | 37 | # xdot for f_impl 38 | x_dot = SX.sym('x_dot') 39 | y_dot = SX.sym('y_dot') 40 | z_dot = SX.sym('z_dot') 41 | u_dot = SX.sym('u_dot') 42 | v_dot = SX.sym('v_dot') 43 | w_dot = SX.sym('w_dot') 44 | phi_dot = SX.sym('phi_dot') 45 | theta_dot = SX.sym('theta_dot') 46 | sym_xdot = vertcat(x_dot,y_dot,z_dot,u_dot,v_dot,w_dot,phi_dot,theta_dot) 47 | 48 | # dynamics 49 | dx = u 50 | dy = v 51 | dz = w 52 | du = (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi)) * a_z 53 | dv = (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi)) * a_z 54 | dw = -g + cos(theta) * cos(phi) * a_z 55 | dphi = (phi_cmd - phi) / tau_phi 56 | dtheta = (theta_cmd - theta) / tau_theta 57 | f_expl = vertcat(dx,dy,dz,du,dv,dw,dphi,dtheta) 58 | 59 | f_impl = sym_xdot - f_expl 60 | 61 | # nonlinear least sqares 62 | cost_y_expr = vertcat(sym_x, sym_u) 63 | #W = block_diag(W_x, W_u) 64 | 65 | model = AcadosModel() 66 | model.f_impl_expr = f_impl 67 | model.f_expl_expr = f_expl 68 | model.x = sym_x 69 | model.xdot = sym_xdot 70 | model.u = sym_u 71 | model.p = sym_p 72 | model.cost_y_expr = cost_y_expr 73 | model.cost_y_expr_e = sym_x 74 | model.name = model_name 75 | 76 | return model 77 | 78 | def main(): 79 | # create ocp object to formulate the OCP 80 | ocp = AcadosOcp() 81 | 82 | # set model 83 | model = export_quadrotor_model() 84 | ocp.model = model 85 | nx = model.x.size()[0] 86 | nu = model.u.size()[0] 87 | nparam = model.p.size()[0] 88 | 89 | # Set prediction size 90 | Tf = 1.0 # Prediction horizon (seconds) 91 | N = 20 # Prediction steps 92 | ocp.dims.N = N 93 | 94 | # set parameters 95 | ocp.parameter_values = np.zeros((nparam, )) 96 | 97 | # set cost 98 | W_x = np.diag([60, 60, 60, 30, 30, 30, 10, 10]) #Q_mat 99 | W_u = np.diag([100, 2000, 2000]) #R_mat 100 | W = block_diag(W_x, W_u) 101 | ocp.cost.W_e = W_x 102 | ocp.cost.W = W 103 | 104 | # V_x = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) 105 | # V_xe = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) 106 | # V_u = np.diag([1.0, 1.0, 1.0]) 107 | # ocp.cost.Vx = V_x 108 | # ocp.cost.Vx_e = V_xe 109 | # ocp.cost.Vu = V_u 110 | 111 | # the 'EXTERNAL' cost type can be used to define general cost terms 112 | # NOTE: This leads to additional (exact) hessian contributions when using GAUSS_NEWTON hessian. 113 | #ocp.cost.cost_type = 'EXTERNAL' 114 | #ocp.cost.cost_type_e = 'EXTERNAL' 115 | ocp.cost.cost_type = 'NONLINEAR_LS' 116 | ocp.cost.cost_type_e = 'NONLINEAR_LS' 117 | 118 | # set constraints 119 | u_min = np.array([0, -math.pi/4, -math.pi/4]) 120 | u_max = np.array([20, math.pi/4, math.pi/4]) 121 | x_min = np.array([-math.pi/4,-math.pi/4]) 122 | x_max = np.array([math.pi/4,math.pi/4]) 123 | ocp.constraints.lbu = u_min 124 | ocp.constraints.ubu = u_max 125 | ocp.constraints.idxbu = np.array([0,1,2]) 126 | ocp.constraints.lbx = x_min 127 | ocp.constraints.ubx = x_max 128 | ocp.constraints.idxbx = np.array([6,7]) 129 | ocp.constraints.x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 130 | 131 | # reference trajectory (will be overwritten later) 132 | x_ref = np.zeros(nx) 133 | ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0]))) 134 | ocp.cost.yref_e = x_ref 135 | 136 | # set options 137 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 138 | # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, 139 | # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP 140 | #ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' 141 | ocp.solver_options.integrator_type = 'ERK' 142 | ocp.solver_options.qp_solver_cond_N = 5 143 | #ocp.solver_options.print_level = 0 144 | ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP 145 | 146 | # set prediction horizon 147 | ocp.solver_options.tf = Tf 148 | ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') 149 | 150 | # simX = np.ndarray((N+1, nx)) 151 | # simU = np.ndarray((N, nu)) 152 | 153 | # status = ocp_solver.solve() 154 | # ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") 155 | 156 | # if status != 0: 157 | # raise Exception(f'acados returned status {status}.') 158 | 159 | if __name__ == '__main__': 160 | main() -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_e_fun_jac_ut_xt.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_e_fun_jac_ut_xt_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | #define casadi_s4 CASADI_PREFIX(s4) 38 | 39 | /* Symbol visibility in DLLs */ 40 | #ifndef CASADI_SYMBOL_EXPORT 41 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 42 | #if defined(STATIC_LINKED) 43 | #define CASADI_SYMBOL_EXPORT 44 | #else 45 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 46 | #endif 47 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 48 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 49 | #else 50 | #define CASADI_SYMBOL_EXPORT 51 | #endif 52 | #endif 53 | 54 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 55 | static const casadi_int casadi_s1[3] = {0, 0, 0}; 56 | static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2}; 57 | static const casadi_int casadi_s3[19] = {8, 8, 0, 1, 2, 3, 4, 5, 6, 7, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 58 | static const casadi_int casadi_s4[3] = {8, 0, 0}; 59 | 60 | /* quadrotor_cost_y_e_fun_jac_ut_xt:(i0[8],i1[],i2[],i3[3])->(o0[8],o1[8x8,8nz],o2[8x0]) */ 61 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 62 | casadi_real a0; 63 | a0=arg[0]? arg[0][0] : 0; 64 | if (res[0]!=0) res[0][0]=a0; 65 | a0=arg[0]? arg[0][1] : 0; 66 | if (res[0]!=0) res[0][1]=a0; 67 | a0=arg[0]? arg[0][2] : 0; 68 | if (res[0]!=0) res[0][2]=a0; 69 | a0=arg[0]? arg[0][3] : 0; 70 | if (res[0]!=0) res[0][3]=a0; 71 | a0=arg[0]? arg[0][4] : 0; 72 | if (res[0]!=0) res[0][4]=a0; 73 | a0=arg[0]? arg[0][5] : 0; 74 | if (res[0]!=0) res[0][5]=a0; 75 | a0=arg[0]? arg[0][6] : 0; 76 | if (res[0]!=0) res[0][6]=a0; 77 | a0=arg[0]? arg[0][7] : 0; 78 | if (res[0]!=0) res[0][7]=a0; 79 | a0=1.; 80 | if (res[1]!=0) res[1][0]=a0; 81 | if (res[1]!=0) res[1][1]=a0; 82 | if (res[1]!=0) res[1][2]=a0; 83 | if (res[1]!=0) res[1][3]=a0; 84 | if (res[1]!=0) res[1][4]=a0; 85 | if (res[1]!=0) res[1][5]=a0; 86 | if (res[1]!=0) res[1][6]=a0; 87 | if (res[1]!=0) res[1][7]=a0; 88 | return 0; 89 | } 90 | 91 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 92 | return casadi_f0(arg, res, iw, w, mem); 93 | } 94 | 95 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_jac_ut_xt_alloc_mem(void) { 96 | return 0; 97 | } 98 | 99 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_jac_ut_xt_init_mem(int mem) { 100 | return 0; 101 | } 102 | 103 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_jac_ut_xt_free_mem(int mem) { 104 | } 105 | 106 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_jac_ut_xt_checkout(void) { 107 | return 0; 108 | } 109 | 110 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_jac_ut_xt_release(int mem) { 111 | } 112 | 113 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_jac_ut_xt_incref(void) { 114 | } 115 | 116 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_e_fun_jac_ut_xt_decref(void) { 117 | } 118 | 119 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;} 120 | 121 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;} 122 | 123 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) { 124 | switch (i) { 125 | default: return 0; 126 | } 127 | } 128 | 129 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) { 130 | switch (i) { 131 | case 0: return "i0"; 132 | case 1: return "i1"; 133 | case 2: return "i2"; 134 | case 3: return "i3"; 135 | default: return 0; 136 | } 137 | } 138 | 139 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) { 140 | switch (i) { 141 | case 0: return "o0"; 142 | case 1: return "o1"; 143 | case 2: return "o2"; 144 | default: return 0; 145 | } 146 | } 147 | 148 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) { 149 | switch (i) { 150 | case 0: return casadi_s0; 151 | case 1: return casadi_s1; 152 | case 2: return casadi_s1; 153 | case 3: return casadi_s2; 154 | default: return 0; 155 | } 156 | } 157 | 158 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) { 159 | switch (i) { 160 | case 0: return casadi_s0; 161 | case 1: return casadi_s3; 162 | case 2: return casadi_s4; 163 | default: return 0; 164 | } 165 | } 166 | 167 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 168 | if (sz_arg) *sz_arg = 4; 169 | if (sz_res) *sz_res = 3; 170 | if (sz_iw) *sz_iw = 0; 171 | if (sz_w) *sz_w = 0; 172 | return 0; 173 | } 174 | 175 | 176 | #ifdef __cplusplus 177 | } /* extern "C" */ 178 | #endif 179 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_fun_jac_ut_xt.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_fun_jac_ut_xt_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | #define casadi_s4 CASADI_PREFIX(s4) 38 | #define casadi_s5 CASADI_PREFIX(s5) 39 | 40 | /* Symbol visibility in DLLs */ 41 | #ifndef CASADI_SYMBOL_EXPORT 42 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 43 | #if defined(STATIC_LINKED) 44 | #define CASADI_SYMBOL_EXPORT 45 | #else 46 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 47 | #endif 48 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 49 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 50 | #else 51 | #define CASADI_SYMBOL_EXPORT 52 | #endif 53 | #endif 54 | 55 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 56 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 57 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 58 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 59 | static const casadi_int casadi_s4[25] = {11, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 3, 4, 5, 6, 7, 8, 9, 10, 0, 1, 2}; 60 | static const casadi_int casadi_s5[3] = {11, 0, 0}; 61 | 62 | /* quadrotor_cost_y_fun_jac_ut_xt:(i0[8],i1[3],i2[],i3[3])->(o0[11],o1[11x11,11nz],o2[11x0]) */ 63 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 64 | casadi_real a0; 65 | a0=arg[0]? arg[0][0] : 0; 66 | if (res[0]!=0) res[0][0]=a0; 67 | a0=arg[0]? arg[0][1] : 0; 68 | if (res[0]!=0) res[0][1]=a0; 69 | a0=arg[0]? arg[0][2] : 0; 70 | if (res[0]!=0) res[0][2]=a0; 71 | a0=arg[0]? arg[0][3] : 0; 72 | if (res[0]!=0) res[0][3]=a0; 73 | a0=arg[0]? arg[0][4] : 0; 74 | if (res[0]!=0) res[0][4]=a0; 75 | a0=arg[0]? arg[0][5] : 0; 76 | if (res[0]!=0) res[0][5]=a0; 77 | a0=arg[0]? arg[0][6] : 0; 78 | if (res[0]!=0) res[0][6]=a0; 79 | a0=arg[0]? arg[0][7] : 0; 80 | if (res[0]!=0) res[0][7]=a0; 81 | a0=arg[1]? arg[1][0] : 0; 82 | if (res[0]!=0) res[0][8]=a0; 83 | a0=arg[1]? arg[1][1] : 0; 84 | if (res[0]!=0) res[0][9]=a0; 85 | a0=arg[1]? arg[1][2] : 0; 86 | if (res[0]!=0) res[0][10]=a0; 87 | a0=1.; 88 | if (res[1]!=0) res[1][0]=a0; 89 | if (res[1]!=0) res[1][1]=a0; 90 | if (res[1]!=0) res[1][2]=a0; 91 | if (res[1]!=0) res[1][3]=a0; 92 | if (res[1]!=0) res[1][4]=a0; 93 | if (res[1]!=0) res[1][5]=a0; 94 | if (res[1]!=0) res[1][6]=a0; 95 | if (res[1]!=0) res[1][7]=a0; 96 | if (res[1]!=0) res[1][8]=a0; 97 | if (res[1]!=0) res[1][9]=a0; 98 | if (res[1]!=0) res[1][10]=a0; 99 | return 0; 100 | } 101 | 102 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 103 | return casadi_f0(arg, res, iw, w, mem); 104 | } 105 | 106 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_jac_ut_xt_alloc_mem(void) { 107 | return 0; 108 | } 109 | 110 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_jac_ut_xt_init_mem(int mem) { 111 | return 0; 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_jac_ut_xt_free_mem(int mem) { 115 | } 116 | 117 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_jac_ut_xt_checkout(void) { 118 | return 0; 119 | } 120 | 121 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_jac_ut_xt_release(int mem) { 122 | } 123 | 124 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_jac_ut_xt_incref(void) { 125 | } 126 | 127 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_fun_jac_ut_xt_decref(void) { 128 | } 129 | 130 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_fun_jac_ut_xt_n_in(void) { return 4;} 131 | 132 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_fun_jac_ut_xt_n_out(void) { return 3;} 133 | 134 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_fun_jac_ut_xt_default_in(casadi_int i) { 135 | switch (i) { 136 | default: return 0; 137 | } 138 | } 139 | 140 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_fun_jac_ut_xt_name_in(casadi_int i) { 141 | switch (i) { 142 | case 0: return "i0"; 143 | case 1: return "i1"; 144 | case 2: return "i2"; 145 | case 3: return "i3"; 146 | default: return 0; 147 | } 148 | } 149 | 150 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_fun_jac_ut_xt_name_out(casadi_int i) { 151 | switch (i) { 152 | case 0: return "o0"; 153 | case 1: return "o1"; 154 | case 2: return "o2"; 155 | default: return 0; 156 | } 157 | } 158 | 159 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) { 160 | switch (i) { 161 | case 0: return casadi_s0; 162 | case 1: return casadi_s1; 163 | case 2: return casadi_s2; 164 | case 3: return casadi_s1; 165 | default: return 0; 166 | } 167 | } 168 | 169 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) { 170 | switch (i) { 171 | case 0: return casadi_s3; 172 | case 1: return casadi_s4; 173 | case 2: return casadi_s5; 174 | default: return 0; 175 | } 176 | } 177 | 178 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 179 | if (sz_arg) *sz_arg = 4; 180 | if (sz_res) *sz_res = 3; 181 | if (sz_iw) *sz_iw = 0; 182 | if (sz_w) *sz_w = 0; 183 | return 0; 184 | } 185 | 186 | 187 | #ifdef __cplusplus 188 | } /* extern "C" */ 189 | #endif 190 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_cost/quadrotor_cost_y_0_fun_jac_ut_xt.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_cost_y_0_fun_jac_ut_xt_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | #define casadi_s4 CASADI_PREFIX(s4) 38 | #define casadi_s5 CASADI_PREFIX(s5) 39 | 40 | /* Symbol visibility in DLLs */ 41 | #ifndef CASADI_SYMBOL_EXPORT 42 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 43 | #if defined(STATIC_LINKED) 44 | #define CASADI_SYMBOL_EXPORT 45 | #else 46 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 47 | #endif 48 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 49 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 50 | #else 51 | #define CASADI_SYMBOL_EXPORT 52 | #endif 53 | #endif 54 | 55 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 56 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 57 | static const casadi_int casadi_s2[3] = {0, 0, 0}; 58 | static const casadi_int casadi_s3[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 59 | static const casadi_int casadi_s4[25] = {11, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 3, 4, 5, 6, 7, 8, 9, 10, 0, 1, 2}; 60 | static const casadi_int casadi_s5[3] = {11, 0, 0}; 61 | 62 | /* quadrotor_cost_y_0_fun_jac_ut_xt:(i0[8],i1[3],i2[],i3[3])->(o0[11],o1[11x11,11nz],o2[11x0]) */ 63 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 64 | casadi_real a0; 65 | a0=arg[0]? arg[0][0] : 0; 66 | if (res[0]!=0) res[0][0]=a0; 67 | a0=arg[0]? arg[0][1] : 0; 68 | if (res[0]!=0) res[0][1]=a0; 69 | a0=arg[0]? arg[0][2] : 0; 70 | if (res[0]!=0) res[0][2]=a0; 71 | a0=arg[0]? arg[0][3] : 0; 72 | if (res[0]!=0) res[0][3]=a0; 73 | a0=arg[0]? arg[0][4] : 0; 74 | if (res[0]!=0) res[0][4]=a0; 75 | a0=arg[0]? arg[0][5] : 0; 76 | if (res[0]!=0) res[0][5]=a0; 77 | a0=arg[0]? arg[0][6] : 0; 78 | if (res[0]!=0) res[0][6]=a0; 79 | a0=arg[0]? arg[0][7] : 0; 80 | if (res[0]!=0) res[0][7]=a0; 81 | a0=arg[1]? arg[1][0] : 0; 82 | if (res[0]!=0) res[0][8]=a0; 83 | a0=arg[1]? arg[1][1] : 0; 84 | if (res[0]!=0) res[0][9]=a0; 85 | a0=arg[1]? arg[1][2] : 0; 86 | if (res[0]!=0) res[0][10]=a0; 87 | a0=1.; 88 | if (res[1]!=0) res[1][0]=a0; 89 | if (res[1]!=0) res[1][1]=a0; 90 | if (res[1]!=0) res[1][2]=a0; 91 | if (res[1]!=0) res[1][3]=a0; 92 | if (res[1]!=0) res[1][4]=a0; 93 | if (res[1]!=0) res[1][5]=a0; 94 | if (res[1]!=0) res[1][6]=a0; 95 | if (res[1]!=0) res[1][7]=a0; 96 | if (res[1]!=0) res[1][8]=a0; 97 | if (res[1]!=0) res[1][9]=a0; 98 | if (res[1]!=0) res[1][10]=a0; 99 | return 0; 100 | } 101 | 102 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 103 | return casadi_f0(arg, res, iw, w, mem); 104 | } 105 | 106 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_jac_ut_xt_alloc_mem(void) { 107 | return 0; 108 | } 109 | 110 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_jac_ut_xt_init_mem(int mem) { 111 | return 0; 112 | } 113 | 114 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_jac_ut_xt_free_mem(int mem) { 115 | } 116 | 117 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_jac_ut_xt_checkout(void) { 118 | return 0; 119 | } 120 | 121 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_jac_ut_xt_release(int mem) { 122 | } 123 | 124 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_jac_ut_xt_incref(void) { 125 | } 126 | 127 | CASADI_SYMBOL_EXPORT void quadrotor_cost_y_0_fun_jac_ut_xt_decref(void) { 128 | } 129 | 130 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;} 131 | 132 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;} 133 | 134 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) { 135 | switch (i) { 136 | default: return 0; 137 | } 138 | } 139 | 140 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) { 141 | switch (i) { 142 | case 0: return "i0"; 143 | case 1: return "i1"; 144 | case 2: return "i2"; 145 | case 3: return "i3"; 146 | default: return 0; 147 | } 148 | } 149 | 150 | CASADI_SYMBOL_EXPORT const char* quadrotor_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) { 151 | switch (i) { 152 | case 0: return "o0"; 153 | case 1: return "o1"; 154 | case 2: return "o2"; 155 | default: return 0; 156 | } 157 | } 158 | 159 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) { 160 | switch (i) { 161 | case 0: return casadi_s0; 162 | case 1: return casadi_s1; 163 | case 2: return casadi_s2; 164 | case 3: return casadi_s1; 165 | default: return 0; 166 | } 167 | } 168 | 169 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) { 170 | switch (i) { 171 | case 0: return casadi_s3; 172 | case 1: return casadi_s4; 173 | case 2: return casadi_s5; 174 | default: return 0; 175 | } 176 | } 177 | 178 | CASADI_SYMBOL_EXPORT int quadrotor_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 179 | if (sz_arg) *sz_arg = 4; 180 | if (sz_res) *sz_res = 3; 181 | if (sz_iw) *sz_iw = 0; 182 | if (sz_w) *sz_w = 0; 183 | return 0; 184 | } 185 | 186 | 187 | #ifdef __cplusplus 188 | } /* extern "C" */ 189 | #endif 190 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/quadrotor_model/quadrotor_expl_vde_adj.c: -------------------------------------------------------------------------------- 1 | /* This file was automatically generated by CasADi 3.6.3. 2 | * It consists of: 3 | * 1) content generated by CasADi runtime: not copyrighted 4 | * 2) template code copied from CasADi source: permissively licensed (MIT-0) 5 | * 3) user code: owned by the user 6 | * 7 | */ 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | /* How to prefix internal symbols */ 13 | #ifdef CASADI_CODEGEN_PREFIX 14 | #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) 15 | #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID 16 | #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) 17 | #else 18 | #define CASADI_PREFIX(ID) quadrotor_expl_vde_adj_ ## ID 19 | #endif 20 | 21 | #include 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | 37 | /* Symbol visibility in DLLs */ 38 | #ifndef CASADI_SYMBOL_EXPORT 39 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 40 | #if defined(STATIC_LINKED) 41 | #define CASADI_SYMBOL_EXPORT 42 | #else 43 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 44 | #endif 45 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 46 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 47 | #else 48 | #define CASADI_SYMBOL_EXPORT 49 | #endif 50 | #endif 51 | 52 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 53 | static const casadi_int casadi_s1[7] = {3, 1, 0, 3, 0, 1, 2}; 54 | static const casadi_int casadi_s2[15] = {11, 1, 0, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; 55 | 56 | /* quadrotor_expl_vde_adj:(i0[8],i1[8],i2[3],i3[3])->(o0[11]) */ 57 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 58 | casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a3, a4, a5, a6, a7, a8, a9; 59 | a0=0.; 60 | if (res[0]!=0) res[0][0]=a0; 61 | if (res[0]!=0) res[0][1]=a0; 62 | if (res[0]!=0) res[0][2]=a0; 63 | a0=arg[1]? arg[1][0] : 0; 64 | if (res[0]!=0) res[0][3]=a0; 65 | a0=arg[1]? arg[1][1] : 0; 66 | if (res[0]!=0) res[0][4]=a0; 67 | a0=arg[1]? arg[1][2] : 0; 68 | if (res[0]!=0) res[0][5]=a0; 69 | a0=arg[0]? arg[0][6] : 0; 70 | a1=cos(a0); 71 | a2=arg[3]? arg[3][2] : 0; 72 | a3=sin(a2); 73 | a4=arg[2]? arg[2][0] : 0; 74 | a5=arg[1]? arg[1][3] : 0; 75 | a6=(a4*a5); 76 | a7=(a3*a6); 77 | a1=(a1*a7); 78 | a7=arg[1]? arg[1][6] : 0; 79 | a8=arg[3]? arg[3][0] : 0; 80 | a7=(a7/a8); 81 | a8=sin(a0); 82 | a9=arg[0]? arg[0][7] : 0; 83 | a10=cos(a9); 84 | a11=arg[1]? arg[1][5] : 0; 85 | a12=(a4*a11); 86 | a13=(a10*a12); 87 | a8=(a8*a13); 88 | a8=(a7+a8); 89 | a13=cos(a0); 90 | a14=cos(a2); 91 | a15=arg[1]? arg[1][4] : 0; 92 | a4=(a4*a15); 93 | a16=(a14*a4); 94 | a13=(a13*a16); 95 | a8=(a8+a13); 96 | a13=sin(a0); 97 | a16=sin(a9); 98 | a17=sin(a2); 99 | a4=(a17*a4); 100 | a18=(a16*a4); 101 | a13=(a13*a18); 102 | a8=(a8+a13); 103 | a1=(a1-a8); 104 | a8=sin(a0); 105 | a13=sin(a9); 106 | a2=cos(a2); 107 | a6=(a2*a6); 108 | a18=(a13*a6); 109 | a8=(a8*a18); 110 | a1=(a1-a8); 111 | if (res[0]!=0) res[0][6]=a1; 112 | a1=cos(a9); 113 | a8=cos(a0); 114 | a4=(a8*a4); 115 | a1=(a1*a4); 116 | a4=arg[1]? arg[1][7] : 0; 117 | a18=arg[3]? arg[3][1] : 0; 118 | a4=(a4/a18); 119 | a18=sin(a9); 120 | a19=cos(a0); 121 | a12=(a19*a12); 122 | a18=(a18*a12); 123 | a18=(a4+a18); 124 | a1=(a1-a18); 125 | a9=cos(a9); 126 | a18=cos(a0); 127 | a6=(a18*a6); 128 | a9=(a9*a6); 129 | a1=(a1+a9); 130 | if (res[0]!=0) res[0][7]=a1; 131 | a10=(a10*a19); 132 | a10=(a10*a11); 133 | a8=(a8*a16); 134 | a8=(a8*a17); 135 | a17=sin(a0); 136 | a17=(a17*a14); 137 | a8=(a8-a17); 138 | a8=(a8*a15); 139 | a10=(a10+a8); 140 | a18=(a18*a13); 141 | a18=(a18*a2); 142 | a0=sin(a0); 143 | a0=(a0*a3); 144 | a18=(a18+a0); 145 | a18=(a18*a5); 146 | a10=(a10+a18); 147 | if (res[0]!=0) res[0][8]=a10; 148 | if (res[0]!=0) res[0][9]=a7; 149 | if (res[0]!=0) res[0][10]=a4; 150 | return 0; 151 | } 152 | 153 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 154 | return casadi_f0(arg, res, iw, w, mem); 155 | } 156 | 157 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_adj_alloc_mem(void) { 158 | return 0; 159 | } 160 | 161 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_adj_init_mem(int mem) { 162 | return 0; 163 | } 164 | 165 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_adj_free_mem(int mem) { 166 | } 167 | 168 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_adj_checkout(void) { 169 | return 0; 170 | } 171 | 172 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_adj_release(int mem) { 173 | } 174 | 175 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_adj_incref(void) { 176 | } 177 | 178 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_adj_decref(void) { 179 | } 180 | 181 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_vde_adj_n_in(void) { return 4;} 182 | 183 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_vde_adj_n_out(void) { return 1;} 184 | 185 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_expl_vde_adj_default_in(casadi_int i) { 186 | switch (i) { 187 | default: return 0; 188 | } 189 | } 190 | 191 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_vde_adj_name_in(casadi_int i) { 192 | switch (i) { 193 | case 0: return "i0"; 194 | case 1: return "i1"; 195 | case 2: return "i2"; 196 | case 3: return "i3"; 197 | default: return 0; 198 | } 199 | } 200 | 201 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_vde_adj_name_out(casadi_int i) { 202 | switch (i) { 203 | case 0: return "o0"; 204 | default: return 0; 205 | } 206 | } 207 | 208 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_vde_adj_sparsity_in(casadi_int i) { 209 | switch (i) { 210 | case 0: return casadi_s0; 211 | case 1: return casadi_s0; 212 | case 2: return casadi_s1; 213 | case 3: return casadi_s1; 214 | default: return 0; 215 | } 216 | } 217 | 218 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_vde_adj_sparsity_out(casadi_int i) { 219 | switch (i) { 220 | case 0: return casadi_s2; 221 | default: return 0; 222 | } 223 | } 224 | 225 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 226 | if (sz_arg) *sz_arg = 4; 227 | if (sz_res) *sz_res = 1; 228 | if (sz_iw) *sz_iw = 0; 229 | if (sz_w) *sz_w = 0; 230 | return 0; 231 | } 232 | 233 | 234 | #ifdef __cplusplus 235 | } /* extern "C" */ 236 | #endif 237 | -------------------------------------------------------------------------------- /airo_control/acados_scripts/c_generated_code/acados_solver_quadrotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, 3 | * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, 4 | * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, 5 | * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl 6 | * 7 | * This file is part of acados. 8 | * 9 | * The 2-Clause BSD License 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * 14 | * 1. Redistributions of source code must retain the above copyright notice, 15 | * this list of conditions and the following disclaimer. 16 | * 17 | * 2. Redistributions in binary form must reproduce the above copyright notice, 18 | * this list of conditions and the following disclaimer in the documentation 19 | * and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE.; 32 | */ 33 | 34 | #ifndef ACADOS_SOLVER_quadrotor_H_ 35 | #define ACADOS_SOLVER_quadrotor_H_ 36 | 37 | #include "acados/utils/types.h" 38 | 39 | #include "acados_c/ocp_nlp_interface.h" 40 | #include "acados_c/external_function_interface.h" 41 | 42 | #define QUADROTOR_NX 8 43 | #define QUADROTOR_NZ 0 44 | #define QUADROTOR_NU 3 45 | #define QUADROTOR_NP 3 46 | #define QUADROTOR_NBX 2 47 | #define QUADROTOR_NBX0 8 48 | #define QUADROTOR_NBU 3 49 | #define QUADROTOR_NSBX 0 50 | #define QUADROTOR_NSBU 0 51 | #define QUADROTOR_NSH 0 52 | #define QUADROTOR_NSG 0 53 | #define QUADROTOR_NSPHI 0 54 | #define QUADROTOR_NSHN 0 55 | #define QUADROTOR_NSGN 0 56 | #define QUADROTOR_NSPHIN 0 57 | #define QUADROTOR_NSBXN 0 58 | #define QUADROTOR_NS 0 59 | #define QUADROTOR_NSN 0 60 | #define QUADROTOR_NG 0 61 | #define QUADROTOR_NBXN 0 62 | #define QUADROTOR_NGN 0 63 | #define QUADROTOR_NY0 11 64 | #define QUADROTOR_NY 11 65 | #define QUADROTOR_NYN 8 66 | #define QUADROTOR_N 20 67 | #define QUADROTOR_NH 0 68 | #define QUADROTOR_NPHI 0 69 | #define QUADROTOR_NHN 0 70 | #define QUADROTOR_NPHIN 0 71 | #define QUADROTOR_NR 0 72 | 73 | #ifdef __cplusplus 74 | extern "C" { 75 | #endif 76 | 77 | 78 | // ** capsule for solver data ** 79 | typedef struct quadrotor_solver_capsule 80 | { 81 | // acados objects 82 | ocp_nlp_in *nlp_in; 83 | ocp_nlp_out *nlp_out; 84 | ocp_nlp_out *sens_out; 85 | ocp_nlp_solver *nlp_solver; 86 | void *nlp_opts; 87 | ocp_nlp_plan_t *nlp_solver_plan; 88 | ocp_nlp_config *nlp_config; 89 | ocp_nlp_dims *nlp_dims; 90 | 91 | // number of expected runtime parameters 92 | unsigned int nlp_np; 93 | 94 | /* external functions */ 95 | // dynamics 96 | 97 | external_function_param_casadi *forw_vde_casadi; 98 | external_function_param_casadi *expl_ode_fun; 99 | 100 | 101 | 102 | 103 | // cost 104 | 105 | external_function_param_casadi *cost_y_fun; 106 | external_function_param_casadi *cost_y_fun_jac_ut_xt; 107 | external_function_param_casadi *cost_y_hess; 108 | 109 | 110 | 111 | external_function_param_casadi cost_y_0_fun; 112 | external_function_param_casadi cost_y_0_fun_jac_ut_xt; 113 | external_function_param_casadi cost_y_0_hess; 114 | 115 | 116 | 117 | external_function_param_casadi cost_y_e_fun; 118 | external_function_param_casadi cost_y_e_fun_jac_ut_xt; 119 | external_function_param_casadi cost_y_e_hess; 120 | 121 | 122 | // constraints 123 | 124 | 125 | 126 | 127 | } quadrotor_solver_capsule; 128 | 129 | ACADOS_SYMBOL_EXPORT quadrotor_solver_capsule * quadrotor_acados_create_capsule(void); 130 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_free_capsule(quadrotor_solver_capsule *capsule); 131 | 132 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_create(quadrotor_solver_capsule * capsule); 133 | 134 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_reset(quadrotor_solver_capsule* capsule, int reset_qp_solver_mem); 135 | 136 | /** 137 | * Generic version of quadrotor_acados_create which allows to use a different number of shooting intervals than 138 | * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code 139 | * generation, the time-steps from code generation is used. 140 | */ 141 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_create_with_discretization(quadrotor_solver_capsule * capsule, int n_time_steps, double* new_time_steps); 142 | /** 143 | * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the 144 | * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. 145 | */ 146 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_update_time_steps(quadrotor_solver_capsule * capsule, int N, double* new_time_steps); 147 | /** 148 | * This function is used for updating an already initialized solver with a different number of qp_cond_N. 149 | */ 150 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_update_qp_solver_cond_N(quadrotor_solver_capsule * capsule, int qp_solver_cond_N); 151 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_update_params(quadrotor_solver_capsule * capsule, int stage, double *value, int np); 152 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_update_params_sparse(quadrotor_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); 153 | 154 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_solve(quadrotor_solver_capsule * capsule); 155 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_free(quadrotor_solver_capsule * capsule); 156 | ACADOS_SYMBOL_EXPORT void quadrotor_acados_print_stats(quadrotor_solver_capsule * capsule); 157 | ACADOS_SYMBOL_EXPORT int quadrotor_acados_custom_update(quadrotor_solver_capsule* capsule, double* data, int data_len); 158 | 159 | 160 | ACADOS_SYMBOL_EXPORT ocp_nlp_in *quadrotor_acados_get_nlp_in(quadrotor_solver_capsule * capsule); 161 | ACADOS_SYMBOL_EXPORT ocp_nlp_out *quadrotor_acados_get_nlp_out(quadrotor_solver_capsule * capsule); 162 | ACADOS_SYMBOL_EXPORT ocp_nlp_out *quadrotor_acados_get_sens_out(quadrotor_solver_capsule * capsule); 163 | ACADOS_SYMBOL_EXPORT ocp_nlp_solver *quadrotor_acados_get_nlp_solver(quadrotor_solver_capsule * capsule); 164 | ACADOS_SYMBOL_EXPORT ocp_nlp_config *quadrotor_acados_get_nlp_config(quadrotor_solver_capsule * capsule); 165 | ACADOS_SYMBOL_EXPORT void *quadrotor_acados_get_nlp_opts(quadrotor_solver_capsule * capsule); 166 | ACADOS_SYMBOL_EXPORT ocp_nlp_dims *quadrotor_acados_get_nlp_dims(quadrotor_solver_capsule * capsule); 167 | ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *quadrotor_acados_get_nlp_plan(quadrotor_solver_capsule * capsule); 168 | 169 | #ifdef __cplusplus 170 | } /* extern "C" */ 171 | #endif 172 | 173 | #endif // ACADOS_SOLVER_quadrotor_H_ 174 | -------------------------------------------------------------------------------- /airo_control/src/controller/mpc.cpp: -------------------------------------------------------------------------------- 1 | #include "airo_control/controller/mpc.h" 2 | 3 | MPC::MPC(ros::NodeHandle& nh){ 4 | // Get MPC parameters 5 | nh.getParam("airo_control_node/mpc/pub_debug",param.pub_debug); 6 | nh.getParam("airo_control_node/mpc/enable_preview",param.enable_preview); 7 | nh.getParam("airo_control_node/mpc/enable_thrust_model",param.enable_thrust_model); 8 | nh.getParam("airo_control_node/mpc/hover_thrust",param.hover_thrust); 9 | nh.getParam("airo_control_node/mpc/tau_phi",param.tau_phi); 10 | nh.getParam("airo_control_node/mpc/tau_theta",param.tau_theta); 11 | nh.getParam("airo_control_node/mpc/tau_psi",param.tau_psi); 12 | nh.getParam("airo_control_node/mpc/diag_cost_x",param.diag_cost_x); 13 | nh.getParam("airo_control_node/mpc/diag_cost_u",param.diag_cost_u); 14 | nh.getParam("airo_control_node/mpc/diag_cost_xn",param.diag_cost_xn); 15 | 16 | if (param.enable_thrust_model){ 17 | nh.getParam("airo_control_node/thrust_model/mass",thrust_model.mass); 18 | nh.getParam("airo_control_node/thrust_model/K1",thrust_model.K1); 19 | nh.getParam("airo_control_node/thrust_model/K2",thrust_model.K2); 20 | nh.getParam("airo_control_node/thrust_model/K3",thrust_model.K3); 21 | } 22 | 23 | // Set publishers 24 | debug_pub = nh.advertise("/airo_control/mpc/debug",1); 25 | 26 | // Initialize MPC 27 | int create_status = 1; 28 | create_status = quadrotor_acados_create(mpc_capsule); 29 | if (create_status != 0){ 30 | ROS_INFO_STREAM("acados_create() returned status " << create_status << ". Exiting." << std::endl); 31 | exit(1); 32 | } 33 | 34 | for(unsigned int i=0; i < QUADROTOR_NU; i++) acados_out.u0[i] = 0.0; 35 | for(unsigned int i=0; i < QUADROTOR_NX; i++) acados_in.x0[i] = 0.0; 36 | 37 | // Set weights 38 | set_intermediate_weights(param.diag_cost_x,param.diag_cost_u); 39 | set_terminal_weights(param.diag_cost_xn); 40 | } 41 | 42 | mavros_msgs::AttitudeTarget MPC::solve(const geometry_msgs::PoseStamped& current_pose, const geometry_msgs::TwistStamped& current_twist, const geometry_msgs::AccelStamped& current_accel, const airo_message::ReferenceStamped& ref, const sensor_msgs::BatteryState& battery_state){ 43 | // Resize ref to fit prediction horizon 44 | airo_message::ReferencePreview ref_preview; 45 | ref_preview.header = ref.header; 46 | ref_preview.ref_preview.resize(QUADROTOR_N+1); 47 | for (int i = 0; i < QUADROTOR_N+1; i++){ 48 | ref_preview.ref_preview[i].pose = ref.ref.pose; 49 | ref_preview.ref_preview[i].twist = ref.ref.twist; 50 | ref_preview.ref_preview[i].accel = ref.ref.accel; 51 | } 52 | return MPC::solve(current_pose,current_twist,current_accel,ref_preview,battery_state); 53 | } 54 | 55 | mavros_msgs::AttitudeTarget MPC::solve(const geometry_msgs::PoseStamped& current_pose, const geometry_msgs::TwistStamped& current_twist, const geometry_msgs::AccelStamped& current_accel, const airo_message::ReferencePreview& ref_preview, const sensor_msgs::BatteryState& battery_state){ 56 | // Set reference 57 | ref_euler = BASE_CONTROLLER::q2rpy(ref_preview.ref_preview[0].pose.orientation); 58 | for (int i = 0; i < QUADROTOR_N+1; i++){ 59 | acados_in.yref[i][0] = ref_preview.ref_preview[i].pose.position.x; 60 | acados_in.yref[i][1] = ref_preview.ref_preview[i].pose.position.y; 61 | acados_in.yref[i][2] = ref_preview.ref_preview[i].pose.position.z; 62 | acados_in.yref[i][3] = ref_preview.ref_preview[i].twist.linear.x; 63 | acados_in.yref[i][4] = ref_preview.ref_preview[i].twist.linear.y; 64 | acados_in.yref[i][5] = ref_preview.ref_preview[i].twist.linear.z; 65 | acados_in.yref[i][6] = 0; 66 | acados_in.yref[i][7] = 0; 67 | acados_in.yref[i][8] = g; 68 | acados_in.yref[i][9] = 0; 69 | acados_in.yref[i][10] = 0; 70 | ocp_nlp_cost_model_set(mpc_capsule->nlp_config, mpc_capsule->nlp_dims, mpc_capsule->nlp_in, i, "yref", acados_in.yref[i]); 71 | } 72 | 73 | // Set initial states 74 | current_euler = BASE_CONTROLLER::q2rpy(current_pose.pose.orientation); 75 | acados_in.x0[x] = current_pose.pose.position.x; 76 | acados_in.x0[y] = current_pose.pose.position.y; 77 | acados_in.x0[z] = current_pose.pose.position.z; 78 | acados_in.x0[u] = current_twist.twist.linear.x; 79 | acados_in.x0[v] = current_twist.twist.linear.y; 80 | acados_in.x0[w] = current_twist.twist.linear.z; 81 | acados_in.x0[phi] = current_euler.x(); 82 | acados_in.x0[theta] = current_euler.y(); 83 | ocp_nlp_constraints_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in, 0, "lbx", acados_in.x0); 84 | ocp_nlp_constraints_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in, 0, "ubx", acados_in.x0); 85 | 86 | // Set parameters 87 | for (int i = 0; i < QUADROTOR_N+1; i++){ 88 | acados_param[i][0] = param.tau_phi; 89 | acados_param[i][1] = param.tau_theta; 90 | 91 | // For yaw prediction 92 | if (i == 0){ 93 | acados_param[i][2] = current_euler.z(); 94 | } 95 | else{ 96 | Eigen::Vector3d dummy_euler = BASE_CONTROLLER::q2rpy(ref_preview.ref_preview[i-1].pose.orientation); 97 | if (dummy_euler.z() - acados_param[i-1][3] > M_PI){ 98 | dummy_euler.z() = dummy_euler.z() - 2*M_PI; 99 | } 100 | else if (dummy_euler.z() - acados_param[i-1][3] < -M_PI){ 101 | dummy_euler.z() = dummy_euler.z() + 2*M_PI; 102 | } 103 | acados_param[i][2] = acados_param[i-1][2] + 1.0/QUADROTOR_N * (dummy_euler.z() - acados_param[i-1][2]) / param.tau_psi; 104 | } 105 | quadrotor_acados_update_params(mpc_capsule,i,acados_param[i],QUADROTOR_NP); 106 | } 107 | 108 | // Solve OCP 109 | acados_status = quadrotor_acados_solve(mpc_capsule); 110 | if (acados_status != 0){ 111 | ROS_INFO_STREAM("acados returned status " << acados_status << std::endl); 112 | MPC::print_debug(); 113 | } 114 | acados_out.status = acados_status; 115 | acados_out.kkt_res = (double)mpc_capsule->nlp_out->inf_norm_res; 116 | 117 | ocp_nlp_get(mpc_capsule->nlp_config, mpc_capsule->nlp_solver, "time_tot", &acados_out.cpu_time); 118 | ocp_nlp_out_get(mpc_capsule->nlp_config, mpc_capsule->nlp_dims, mpc_capsule->nlp_out, 0, "u", (void *)acados_out.u0); 119 | 120 | attitude_target.thrust = inverse_thrust_model(acados_out.u0[0],battery_state.voltage,param,thrust_model); 121 | target_euler.x() = acados_out.u0[1]; 122 | target_euler.y() = acados_out.u0[2]; 123 | target_euler.z() = ref_euler.z(); 124 | 125 | geometry_msgs::Quaternion target_quaternion = BASE_CONTROLLER::rpy2q(target_euler); 126 | attitude_target.orientation = target_quaternion; 127 | 128 | if (param.pub_debug){ 129 | pub_debug(); 130 | } 131 | 132 | return attitude_target; 133 | } 134 | 135 | double MPC::get_hover_thrust(){ 136 | return param.hover_thrust; 137 | } 138 | 139 | bool MPC::set_intermediate_weights(const std::vector&diag_weight_x, const std::vector&diag_weight_u){ 140 | if (diag_weight_x.size() != QUADROTOR_NX || diag_weight_u.size() != QUADROTOR_NU){ 141 | return false; 142 | } 143 | 144 | double w[(QUADROTOR_NX+QUADROTOR_NU)*(QUADROTOR_NX+QUADROTOR_NU)]; 145 | for (int j = 0; j < (QUADROTOR_NX+QUADROTOR_NU)*(QUADROTOR_NX+QUADROTOR_NU); j++){ 146 | w[j] = 0.0; 147 | } 148 | for (int j = 0; j < QUADROTOR_NX; j++){ 149 | w[j + (QUADROTOR_NX+QUADROTOR_NU) * j] = diag_weight_x[j]; 150 | } 151 | for (int j = 0; j < QUADROTOR_NU; j++){ 152 | w[QUADROTOR_NX+j + (QUADROTOR_NX+QUADROTOR_NU) * (QUADROTOR_NX+j)] = diag_weight_u[j]; 153 | } 154 | 155 | for (int i = 0; i < QUADROTOR_N; i++){ 156 | ocp_nlp_cost_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in,i,"W",w); 157 | } 158 | return true; 159 | } 160 | 161 | bool MPC::set_terminal_weights(const std::vector&diag_weight_xn){ 162 | if (diag_weight_xn.size() != QUADROTOR_NX){ 163 | return false; 164 | } 165 | 166 | double w_n[QUADROTOR_NX*QUADROTOR_NX]; 167 | for (int j = 0; j < QUADROTOR_NX*QUADROTOR_NX; j++){ 168 | w_n[j] = 0.0; 169 | } 170 | for (int j = 0; j < QUADROTOR_NX; j++){ 171 | w_n[j + QUADROTOR_NX * j] = diag_weight_xn[j]; 172 | } 173 | 174 | ocp_nlp_cost_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in,QUADROTOR_N,"W",w_n); 175 | return true; 176 | } 177 | 178 | void MPC::pub_debug(){ 179 | std_msgs::Float64MultiArray debug_msg; 180 | debug_msg.layout.dim.push_back(std_msgs::MultiArrayDimension()); 181 | debug_msg.layout.dim[0].size = QUADROTOR_N + 4; 182 | debug_msg.layout.dim[0].stride = 1; 183 | debug_msg.data.clear(); 184 | debug_msg.data.push_back(acados_out.status); 185 | debug_msg.data.push_back(acados_out.kkt_res); 186 | debug_msg.data.push_back(acados_out.cpu_time); 187 | for (size_t i = 0; i < QUADROTOR_N + 1; i++){ 188 | debug_msg.data.push_back(acados_param[i][3]); 189 | } 190 | 191 | debug_pub.publish(debug_msg); 192 | } 193 | 194 | void MPC::print_debug(){ 195 | std::cout << "------------------------------------------------------------------------------" << std::endl; 196 | std::cout << "x_ref: " << acados_in.yref[0][0] << "\ty_ref: " << acados_in.yref[0][1] << "\tz_ref: " << acados_in.yref[0][2] << std::endl; 197 | std::cout << "x_gt: " << acados_in.x0[x] << "\ty_gt: " << acados_in.x0[y] << "\tz_gt: " << acados_in.x0[z] << std::endl; 198 | std::cout << "theta_cmd: " << target_euler.y() << "\tphi_cmd: " << target_euler.x() << "\tpsi_cmd: " << target_euler.z() << std::endl; 199 | std::cout << "theta_gt: " << current_euler.y() << "\tphi_gt: " << current_euler.x() << "\tpsi_gt: " << current_euler.z() << std::endl; 200 | std::cout << "thrust_cmd: " << attitude_target.thrust << "\tsolve_time: "<< acados_out.cpu_time << "\tacados_status: " << acados_out.status << std::endl; 201 | std::cout << "ros_time: " << std::fixed << ros::Time::now().toSec() << std::endl; 202 | std::cout << "------------------------------------------------------------------------------" << std::endl; 203 | } -------------------------------------------------------------------------------- /media/AIRo_PX4_FSM.drawio: -------------------------------------------------------------------------------- 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 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 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 | 98 | 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 | 141 | 142 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AIRo Control Interface 2 | This project provides a PX4 based quadrotor UAV control interfaces that supports customized outter-loop controllers. Currently supported controllers include Model Predictive Control (MPC), Backstepping Control (BS), and Sliding-Mode Control(SMC). By using this package you can achieve functions such as auto takeoff/land, RC transmitter control, and follow external trajectory commands with customized outter-loop controllers. The package can be used with both Gazebo simulation or real-world quadrotors with external positioning systems such as Vicon. 3 | 4 | If you use this package in your research, please kindly cite the follow reference: 5 | ``` 6 | @article{jiang2022neural, 7 | title={Neural network based model predictive control for a quadrotor UAV}, 8 | author={Jiang, Bailun and Li, Boyang and Zhou, Weifeng and Lo, Li-Yu and Chen, Chih-Keng and Wen, Chih-Yung}, 9 | journal={Aerospace}, 10 | volume={9}, 11 | number={8}, 12 | pages={460}, 13 | year={2022}, 14 | publisher={MDPI} 15 | } 16 | ``` 17 | 18 | ## Installation 19 | 20 | It is recommanded to run the package in our docker following instructions [here](https://github.com/HKPolyU-UAV/airo_docker_lib). By doing so, you can skip the installation section. 21 | 22 | **Prerequisities** 23 | * ROS ([ROS noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) recommended) 24 | * [QGroundControl](http://qgroundcontrol.com/) 25 | * [MAVROS](http://wiki.ros.org/mavros) 26 | * [Acados](https://docs.acados.org/installation/index.html) 27 | 28 | First, install Acados at your home directory. If you want to install Acados at other directory, change the acados_include and acados_lib directory written in CMakeLists.txt of airo_control package, and also change ```~/acados``` in the following codes to your customized directory in the following codes. 29 | ``` 30 | cd ~ 31 | git clone https://github.com/acados/acados.git 32 | cd acados 33 | git checkout 568e46c 34 | git submodule update --recursive --init 35 | mkdir build 36 | cd build 37 | cmake -DACADOS_WITH_QPOASES=ON -DACADOS_WITH_OSQP=OFF/ON -DACADOS_INSTALL_DIR=~/acados .. 38 | sudo make install -j4 39 | ``` 40 | 41 | Create a catkin workspace and clone this repository to src folder (ex. ~/airo_control_interface_ws/src) 42 | ``` 43 | mkdir -p ~/airo_control_interface_ws/src 44 | cd ~/airo_control_interface_ws 45 | cd src 46 | git clone https://github.com/HKPolyU-UAV/airo_control_interface.git 47 | cd airo_control_interface 48 | git submodule update --init --recursive 49 | ``` 50 | 51 | Build the package. 52 | ``` 53 | cd ~/airo_control_interface_ws 54 | catkin_make 55 | ``` 56 | 57 | Add acados library path to ```.bashrc```, change `````` to your path, e.g. ```/home/your_username```. Note that do not use ```~``` in ``````. 58 | ``` 59 | echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/acados/lib"' >> ~/.bashrc 60 | echo 'export ACADOS_SOURCE_DIR="/acados"' >> ~/.bashrc 61 | ``` 62 | 63 | Add source ```setup.bash``` file to ```.bashrc```. 64 | ``` 65 | echo 'source ~/airo_control_interface_ws/devel/setup.bash' >> ~/.bashrc 66 | ``` 67 | 68 | ## Running Simulation 69 | 70 | Download and install the PX4 (1.11.0) 71 | ``` 72 | cd ~ 73 | git clone https://github.com/PX4/PX4-Autopilot.git 74 | cd PX4-Autopilot/ 75 | git checkout 71db090 76 | git submodule sync --recursive 77 | git submodule update --init --recursive 78 | bash ./Tools/setup/ubuntu.sh 79 | sudo apt upgrade libignition-math4 #(libignition-math2 for melodic) 80 | ``` 81 | 82 | Start PX4 SITL 83 | ``` 84 | cd ~/PX4-Autopilot/ 85 | make px4_sitl_default gazebo 86 | ``` 87 | 88 | Run MAVROS 89 | ``` 90 | roslaunch airo_control mavros_gazebo.launch 91 | ``` 92 | 93 | Start control interface in new terminal 94 | ``` 95 | roslaunch airo_control fsm_gazebo.launch 96 | ``` 97 | 98 | Run an example mission 99 | ``` 100 | rosrun airo_control example_mission_node 101 | ``` 102 | 103 | Or, you can simply run start.sh in the startup folder. 104 | 105 | ## Usage with RC Transmitter 106 | 107 | 0. **Preparation** 108 | 109 | First, launch mavros and QGC and make sure the quadrotor is connected. To use the transmitter in gazebo simulation, set PX4 parameter COM_RCIN_MODE to "RC and Joystick with fallback", connect RC transmitter via usb serial, calibrate the joysticks in "Joysticks" tab and you should be able to read channel inputs in QGC "Radio" tab. In QGC setup page, you only need to set emergency kill channel and flight mode channel. Three more RC channel will be used by the control interface, which are referred to as FSM channel (channel 5 by default), command channel (channel 6 by default), and reboot channel (channel 8 by default). The FSM channel controls if the interface is activated, the command channel controls if the interface follows external trajecdtory commands, and the reboot channel will reboot FCU so that it is recommended to be set to the stick that can automatically flip back. The channel is enabled if the pwm output is greater than the threshold (1750 by default). 110 | 111 | After this you can launch the control interface by 112 | ``` 113 | roslaunch airo_control fsm_gazebo.launch 114 | ``` 115 | or 116 | ``` 117 | roslaunch airo_control fsm_vicon.launch 118 | ``` 119 | For vicon usage, make sure the vehicle can enter position flight mode before the mission. 120 | 121 | The FSM will be initialized to ```RC_MANUAL``` state, where the control interface is not activated in this mode so that you have full control the quadrotor with the transmitter as usual. In general, it is recommended to use the control interface by choosing to enable or disable command channel before running the mission based on the application scenarios. Although switching command channel during mission is supported, it is not required during common applications and could cause confusions. Therefore, we recommend the following two pipelines to work with this FSM. For more detailed introduction of the FSM please refer to the ```FSM Introduction``` section. 122 | 123 | 1. **Non-command Mode** 124 | 125 | In this mode, you'll be controlling the vehicle with the transmitter joysticks. To use in non-command mode, first disable the command channel and then flip the FSM channel from disable to enable state (which will be referred to as switch FSM channel in the follow text). 126 | 127 | Once the FSM channel is switched, the control interface will detect the vehilce state and act accordingly. If the vehicle is landed and disarmed, the vehicle will arm and accelerate motors for a few seconds and then automatically takeoff to desired height. Once takeoff height is reached, the vehicle will follow the joystick commands. If the vehicle is armed when hovering in the air, the FSM will directly follow joystick commands. 128 | 129 | When vehicle is landed by joystick, the control interface will disarm the UAV and re-initiate for the next mission. 130 | 131 | 2. **Command Mode** 132 | 133 | In this mode, the control interface will follow external commands such as takeoff/land and trajectory setpoints. To use in command mode, first enable the command channel and then enable the FSM channel. Unlike the non-command mode, if the command channel is enabled, switching FSM channel will not automatically takeoff the vehicle. 134 | Then the user can send takeoff trigger ```takeoff_land_trigger = true``` to topic ```/airo_px4/takeoff_land_trigger``` and the vehicle will takeoff to desired height. After auto takeoff, the vehicle will hover and publish indicator ```is_waiting_for_command = true``` to topic “/airo_px4/fsm_info" to indicate that the user can send trajectory commands to the control interface. Then, the vehicle will follow commands published to ```/airo_control/setpoint``` (or ```/airo_control/setpoint_preview``` if MPC preview is used). If you stop sending commands, the vehicle will hover at current position. After this you can land and disarm the vehicle by sending ```takeoff_land_trigger = false``` to the same topic. 135 | 136 | ## FSM Introduction 137 | 138 | The control interface uses a finite state machine (FSM) to control the UAV with the detials introduced below. 139 | 140 | 141 | 142 | 1. **RC_MANUAL** 143 | 144 | In this state, the FSM is disabled and the quadrotor operates at manual modes (i.e. position,altitude, and stabilize) using the embedded PID controllers in PX4 firmware. This is the only state that PX4 offboard is disabled and the user have full control over RC transmitter using the embedded PX4 controller. When vehicle is landed, you can use reboot channel to reboot the FCU. 145 | 146 | The FSM is initialized with this state and will go back to it if the vehicle is disarmed, killed, lost connection, or localization message is timed-out. 147 | 148 | 2. **AUTO_TAKEOFF** 149 | 150 | In this state, the vehicle will perform auto takeoff operation. The vehicle will slowly accelerate motors for several seconds to warn others and then takeoff to ```takeoff_height``` at ```takeoff_land_speed```. This state can be entered from ```RC_MANUAL``` state in two conditions. First condition is if command channel is disabled && the vehicle is landed && the FSM channel is switched. Second condition is if command channel is enabled && FSM is enabled && takeoff trigger is received. Once the target height is reached, the FSM will enter ```AUTO_HOVER``` state. 151 | 152 | 3. **AUTO_HOVER** 153 | 154 | In this state, the vehicle will follow the commend of RC transmitter joysticks, which is similar to the PX4 position flight mode. This state can be entered after ```AUTO_TAKEOFF``` or if the FSM channel switched during manual flight using ```RC_MANUAL```. If the vehicle is landed with joystick commands, the FSM will disarm the vehicle and jump back to ```RC_MANUAL``` state. 155 | 156 | If command channel is enabled, the FSM will send ```is_waiting_for_command = true``` to topic ```/airo_px4/fsm_info``` to indicate that the vehicle is waiting for external commands. 157 | 158 | 4. **AUTO_LAND** 159 | 160 | In this state, the vehicle will automatically land and disarm at current x&y position. This state can only be entered when command channel enabled and land command is received. 161 | 162 | 5. **POS_COMMAND** 163 | 164 | In this state, the vehicle will follow external position command subscribed from ```/airo_px4/setpoint``` (or ```/airo_control/setpoint_preview``` if MPC preview is used). This state can be entered from ```AUTO_HOVER``` mode if command channel enabled && position command is received. 165 | 166 | Note that in all states, the position reference given to the controller is confined by the safety constraints set in ```.yaml``` file 167 | 168 | ## Parameters 169 | 170 | 1. **fsm.yaml** 171 | 172 | ```pose_topic```, ```twist_topic```: topics that publish pose & twist messages. 173 | 174 | ```controller_type```: choice of customized outter-loop controller, currently support ```mpc```, ```backstepping```, and ```slidingmode```. 175 | 176 | ```fsm_frequency```: running frequency of control interface and the outter-loop controller. 177 | 178 | ```state_timeout```, ```rc_timeout```, ```odom_timeout```, ```command_timeout```: timeout duration in seconds to decide whether the corresponding messages are received. 179 | 180 | ```motor_speedup_time```: duration in seconds to speedup motor before auto takeoff. 181 | 182 | ```reject_takeoff_twist_threshold```: reject auto takeoff if the vehicle twist is too high. 183 | 184 | ```hover_max_velocity```, ```hover_max_yaw_rate```: maximum translational speed (m/s) and yaw rate (rad/s) commanded by joysticks. 185 | 186 | ```safety_volumn```: min_x, max_x, min_y, max_y, and max_z in meters. Note that min_z is not confined. 187 | 188 | ```without_rc```: set to true if you want to use control interface without rc transmitter. If this set to true, the FSM channel and command channel are enabled by default and you can directly control the vehicle with external commands. If this set to false, the vehicle won't listen to external commands until the transmitter is connected. 189 | 190 | The rest of the parameter list should be self-explanatory. Note that the channel number parameter corresponds to the number shown in QGC. 191 | 192 | 2. **mpc.yaml** 193 | 194 | ```tau_phi```, ```tau_theta```, ```tau_psi```: estimated inner loop dynamics for roll, pitch, and yaw 195 | 196 | ```diag_cost_x```, ```diag_cost_u```, ```diag_cost_xn```: diagnoal weight matrix for state, control input, and terminal state 197 | 198 | 3. **backstepping.yaml** 199 | 200 | ```k_x1```, ```k_y1```, ```k_z1```: position control gains for corresponding axis 201 | 202 | ```k_x2```, ```k_y2```, ```k_z2```: velocity control gains for corresponding axis 203 | 204 | 4. **slidingmode.yaml** 205 | 206 | ```k_xe```, ```k_ye```, ```k_ze```: position gains for corresponding axis 207 | 208 | ```k_xs```, ```k_ys```, ```k_zs```: sliding surface control gains for corresponding axis 209 | 210 | ```k_xt```, ```k_yt```, ```k_zt```: tanh function parameters for corresponding axis 211 | 212 | ## Advanced Usage 213 | 214 | We've developed another package named by [airo_trajectory](https://github.com/HKPolyU-UAV/airo_trajectory) based on this control interface. The package provides a trajectory server to command the vehicle to takeoff/land or follow certain trajectories such as fixed points or polynomial trajectories defined within ```.txt``` file. 215 | 216 | ## Generate MPC Solver 217 | 218 | The python scripts used to generate MPC solver is included in ```~/airo_control_interface_ws/src/airo_control_interface/airo_control/acados_scripts/quadrotor_model.py```. If you want to make modifications and generate MPC solver by your own, follow these instructions. 219 | 220 | Install python 3.7 221 | ``` 222 | sudo add-apt-repository ppa:deadsnakes/ppa 223 | sudo apt update 224 | sudo apt install python3.7 225 | ``` 226 | 227 | Install python dependencies 228 | ``` 229 | python3 -m pip install pip 230 | sudo pip3 install numpy matplotlib scipy future-fstrings casadi>=3.5.1 setuptools 231 | sudo apt-get install python3.7-tk 232 | pip install -e ~/acados/interfaces/acados_template 233 | ``` 234 | 235 | Generate solver, note that for the first run, you should setup Tera renderer automatically by following the instructions. 236 | ``` 237 | cd ~/airo_control_interface_ws/src/airo_control_interface/airo_control/acados_scripts 238 | python3 generate_c_code.py 239 | ``` 240 | 241 | ## MPC System Identification 242 | 243 | To use the control framework with MPC, the quadrotor model parameters should first be identified, which includes hover thrust and inner control loop dynamics for pitch, roll, and yaw movements. In order to do this, first make sure that the quadrotor can be used in position flight mode, then launch ```system_id.launch``` in ```airo_control``` package. The quadrotor will automatically takeoff, perform maneuvers in all axes, and land. Once it's landed, the identified parameters will be displayed in terminal and you can save it in ```.yaml``` file by input ```y```. 244 | -------------------------------------------------------------------------------- /airo_control/src/system_identification.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | double hover_thrust, tau_phi, tau_theta, tau_psi; 23 | double takeoff_height = 1.0; 24 | double yaw_ref = M_PI/4; 25 | std::vector thrust,tau_phi_diff,tau_phi_rate,tau_theta_diff,tau_theta_rate,tau_psi_diff,tau_psi_rate; 26 | bool hover_thrust_id = false; 27 | bool tau_phi_id = false; 28 | bool tau_theta_id = false; 29 | bool tau_psi_id = false; 30 | bool local_pose_received = false; 31 | ros::Time last_state_time; 32 | mavros_msgs::State current_state; 33 | geometry_msgs::PoseStamped local_pose,takeoff_pose,x_maneuver_pose,y_maneuver_pose,yaw_maneuver_pose; 34 | std::string package_path = ros::package::getPath("airo_control"); 35 | std::string POSE_TOPIC, BACKSTEPPING_FILE, SLIDINGMODE_FILE, MPC_FILE; 36 | tf::Quaternion tf_quaternion; 37 | 38 | enum State{ 39 | TAKEOFF, 40 | HOVER, 41 | X_MANEUVER, 42 | Y_MANEUVER, 43 | YAW_MANEUVER, 44 | LAND, 45 | FINISH 46 | }; 47 | 48 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 49 | current_state = *msg; 50 | } 51 | 52 | void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ 53 | local_pose.header = msg->header; 54 | local_pose.pose = msg->pose; 55 | local_pose_received = true; 56 | } 57 | 58 | Eigen::Vector3d q2rpy(geometry_msgs::Quaternion q){ 59 | Eigen::Vector3d euler; 60 | tf::quaternionMsgToTF(q,tf_quaternion); 61 | tf::Matrix3x3(tf_quaternion).getRPY(euler.x(), euler.y(), euler.z()); 62 | return euler; 63 | } 64 | 65 | geometry_msgs::Quaternion rpy2q(Eigen::Vector3d euler){ 66 | geometry_msgs::Quaternion quaternion = tf::createQuaternionMsgFromRollPitchYaw(euler.x(), euler.y(), euler.z()); 67 | return quaternion; 68 | } 69 | 70 | void sync_cb(const geometry_msgs::PoseStampedConstPtr& attitude_msg, const mavros_msgs::AttitudeTargetConstPtr& attitude_target_msg, const geometry_msgs::TwistStampedConstPtr& angular_rate_msg){ 71 | Eigen::Vector3d current_target_euler = q2rpy(attitude_target_msg->orientation); 72 | Eigen::Vector3d current_euler = q2rpy(attitude_msg->pose.orientation); 73 | 74 | if (tau_phi_id){ 75 | tau_phi_diff.push_back(current_target_euler.x() - current_euler.x()); 76 | tau_phi_rate.push_back(angular_rate_msg->twist.angular.x); 77 | }else if (tau_theta_id){ 78 | tau_theta_diff.push_back(current_target_euler.y() - current_euler.y()); 79 | tau_theta_rate.push_back(angular_rate_msg->twist.angular.y); 80 | }else if (tau_psi_id){ 81 | tau_psi_diff.push_back(current_target_euler.z() - current_euler.z()); 82 | tau_psi_rate.push_back(angular_rate_msg->twist.angular.z); 83 | } 84 | } 85 | 86 | void hover_thrust_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){ 87 | if (hover_thrust_id){ 88 | thrust.push_back(msg->thrust); 89 | } 90 | } 91 | 92 | bool target_reached(const geometry_msgs::PoseStamped& msg){ 93 | return sqrt(pow(msg.pose.position.x - local_pose.pose.position.x,2)+pow(msg.pose.position.y - local_pose.pose.position.y,2) 94 | +pow(msg.pose.position.z - local_pose.pose.position.z,2)) < 0.25; 95 | } 96 | 97 | void update_x_maneuver(){ 98 | x_maneuver_pose.pose.position.x = takeoff_pose.pose.position.x + 1.5*sin(2*(ros::Time::now().toSec() - last_state_time.toSec())); 99 | x_maneuver_pose.pose.position.y = takeoff_pose.pose.position.y; 100 | x_maneuver_pose.pose.position.z = takeoff_pose.pose.position.z; 101 | x_maneuver_pose.pose.orientation.w = 1; 102 | x_maneuver_pose.pose.orientation.x = 0; 103 | x_maneuver_pose.pose.orientation.y = 0; 104 | x_maneuver_pose.pose.orientation.z = 0; 105 | } 106 | 107 | void update_y_maneuver(){ 108 | y_maneuver_pose.pose.position.x = takeoff_pose.pose.position.x; 109 | y_maneuver_pose.pose.position.y = takeoff_pose.pose.position.y + 1.5*sin(2*(ros::Time::now().toSec() - last_state_time.toSec())); 110 | y_maneuver_pose.pose.position.z = takeoff_pose.pose.position.z; 111 | y_maneuver_pose.pose.orientation.w = 1; 112 | y_maneuver_pose.pose.orientation.x = 0; 113 | y_maneuver_pose.pose.orientation.y = 0; 114 | y_maneuver_pose.pose.orientation.z = 0; 115 | } 116 | 117 | void update_yaw_maneuver(){ 118 | Eigen::Vector3d euler = q2rpy(local_pose.pose.orientation); 119 | if (euler.z() > M_PI/4 - M_PI/16 && euler.z() < M_PI/4 + M_PI/16 && yaw_ref == M_PI/4){ 120 | yaw_ref = -M_PI/4; 121 | } 122 | else if (euler.z() > -M_PI/4 - M_PI/16 && euler.z() < -M_PI/4 + M_PI/16 && yaw_ref == -M_PI/4){ 123 | yaw_ref = M_PI/4; 124 | } 125 | Eigen::Vector3d euler_ref{0,0,yaw_ref}; 126 | yaw_maneuver_pose.pose.orientation = rpy2q(euler_ref); 127 | yaw_maneuver_pose.pose.position.x = takeoff_pose.pose.position.x; 128 | yaw_maneuver_pose.pose.position.y = takeoff_pose.pose.position.y; 129 | yaw_maneuver_pose.pose.position.z = takeoff_pose.pose.position.z; 130 | } 131 | 132 | double linear_regression(std::vector x, std::vector y){ 133 | // y = a*x 134 | double mean_x = std::accumulate(x.begin(),x.end(),0.0)/x.size(); 135 | double mean_y = std::accumulate(y.begin(),y.end(),0.0)/y.size(); 136 | 137 | double numerator = 0.0; 138 | double denominator = 0.0; 139 | for (int i = 0; i < x.size();++i){ 140 | numerator += (x[i] - mean_x) * (y[i] - mean_y); 141 | denominator += (x[i] - mean_x) * (x[i] - mean_x); 142 | } 143 | 144 | double a = numerator / denominator; 145 | return a; 146 | } 147 | 148 | void write_hover_thrust_to_yaml(const std::string& yaml_path){ 149 | YAML::Node yaml_config = YAML::LoadFile(yaml_path); 150 | std::ofstream yaml_file(yaml_path); 151 | yaml_config["hover_thrust"] = hover_thrust; 152 | yaml_file << yaml_config; 153 | yaml_file.close(); 154 | } 155 | void write_innerloop_param_to_yaml(const std::string& yaml_path){ 156 | YAML::Node yaml_config = YAML::LoadFile(yaml_path); 157 | std::ofstream yaml_file(yaml_path); 158 | yaml_config["tau_phi"] = tau_phi; 159 | yaml_config["tau_theta"] = tau_theta; 160 | yaml_config["tau_psi"] = tau_psi; 161 | yaml_file << yaml_config; 162 | yaml_file.close(); 163 | } 164 | 165 | int main(int argc, char **argv){ 166 | 167 | ros::init(argc, argv, "system_identification_node"); 168 | ros::NodeHandle nh; 169 | ros::Rate rate(100); 170 | State state = TAKEOFF; 171 | 172 | nh.getParam("pose_topic", POSE_TOPIC); 173 | nh.getParam("backstepping_file", BACKSTEPPING_FILE); 174 | nh.getParam("mpc_file", MPC_FILE); 175 | nh.getParam("slidingmode_file", SLIDINGMODE_FILE); 176 | std::string backstepping_path = package_path + BACKSTEPPING_FILE; 177 | std::string mpc_path = package_path + MPC_FILE; 178 | std::string slidingmode_path = package_path + SLIDINGMODE_FILE; 179 | 180 | ros::Subscriber state_sub = nh.subscribe("/mavros/state",10,state_cb); 181 | ros::Subscriber local_pose_sub = nh.subscribe(POSE_TOPIC,10,pose_cb); 182 | ros::Subscriber hover_thrust_sub = nh.subscribe("/mavros/setpoint_raw/target_attitude",10,hover_thrust_cb); 183 | ros::Publisher command_pub = nh.advertise("/mavros/setpoint_position/local",10); 184 | ros::ServiceClient arming_client = nh.serviceClient("/mavros/cmd/arming"); 185 | ros::ServiceClient landing_client = nh.serviceClient("/mavros/cmd/land"); 186 | ros::ServiceClient set_mode_client = nh.serviceClient("/mavros/set_mode"); 187 | 188 | message_filters::Subscriber attitude_sub(nh,"/mavros/local_position/pose",10); 189 | message_filters::Subscriber attitude_target_sub(nh,"/mavros/setpoint_raw/target_attitude",10); 190 | message_filters::Subscriber angular_rate_sub(nh,"/mavros/local_position/velocity_local",10); 191 | typedef message_filters::sync_policies::ApproximateTime my_sync_policy; 192 | message_filters::Synchronizer sync(my_sync_policy(10), attitude_sub,attitude_target_sub,angular_rate_sub); 193 | sync.registerCallback(boost::bind(&sync_cb,_1,_2,_3)); 194 | 195 | while(ros::ok() && (!current_state.connected || ! local_pose_received)){ 196 | ros::spinOnce(); 197 | rate.sleep(); 198 | ROS_INFO_THROTTLE(1.0, "Waiting for connection."); 199 | } 200 | 201 | ROS_INFO("Connected!"); 202 | 203 | takeoff_pose.pose.position.x = local_pose.pose.position.x; 204 | takeoff_pose.pose.position.y = local_pose.pose.position.y; 205 | takeoff_pose.pose.position.z = local_pose.pose.position.z + takeoff_height; 206 | takeoff_pose.pose.orientation.w = 1; 207 | takeoff_pose.pose.orientation.x = 0; 208 | takeoff_pose.pose.orientation.y = 0; 209 | takeoff_pose.pose.orientation.z = 0; 210 | 211 | for(int i = 100; ros::ok() && i > 0; --i){ 212 | command_pub.publish(takeoff_pose); 213 | ros::spinOnce(); 214 | rate.sleep(); 215 | } 216 | 217 | mavros_msgs::SetMode offb_set_mode; 218 | offb_set_mode.request.custom_mode = "OFFBOARD"; 219 | 220 | mavros_msgs::CommandBool arm_cmd; 221 | arm_cmd.request.value = true; 222 | 223 | mavros_msgs::CommandTOL land_cmd; 224 | land_cmd.request.yaw = 0; 225 | land_cmd.request.latitude = 0; 226 | land_cmd.request.longitude = 0; 227 | land_cmd.request.altitude = 0; 228 | 229 | ros::Time last_request = ros::Time::now(); 230 | 231 | while(ros::ok()){ 232 | ros::spinOnce(); 233 | 234 | switch(state){ 235 | case TAKEOFF:{ 236 | if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ 237 | if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ 238 | ROS_WARN("Offboard enabled"); 239 | } 240 | last_request = ros::Time::now(); 241 | } else { 242 | if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ 243 | if( arming_client.call(arm_cmd) && arm_cmd.response.success){ 244 | ROS_WARN("Vehicle armed"); 245 | } 246 | last_request = ros::Time::now(); 247 | } 248 | } 249 | 250 | command_pub.publish(takeoff_pose); 251 | 252 | if(target_reached(takeoff_pose)){ 253 | state = HOVER; 254 | last_state_time = ros::Time::now(); 255 | hover_thrust_id = true; 256 | } 257 | 258 | break; 259 | } 260 | 261 | case HOVER:{ 262 | command_pub.publish(takeoff_pose); 263 | if (ros::Time::now().toSec() - last_state_time.toSec() > 10.0){ 264 | hover_thrust_id = false; 265 | hover_thrust = std::accumulate(thrust.begin(),thrust.end(),0.0) / thrust.size(); 266 | state = X_MANEUVER; 267 | tau_theta_id = true; 268 | last_state_time = ros::Time::now(); 269 | } 270 | break; 271 | } 272 | 273 | case X_MANEUVER:{ 274 | if (ros::Time::now().toSec() - last_state_time.toSec() > 15.0){ 275 | tau_theta_id = false; 276 | state = Y_MANEUVER; 277 | tau_phi_id = true; 278 | last_state_time = ros::Time::now(); 279 | while(ros::ok()){ 280 | command_pub.publish(takeoff_pose); 281 | if (target_reached(takeoff_pose)){ 282 | break; 283 | } 284 | ros::spinOnce(); 285 | ros::Duration(rate).sleep(); 286 | } 287 | } 288 | else{ 289 | update_x_maneuver(); 290 | command_pub.publish(x_maneuver_pose); 291 | } 292 | break; 293 | } 294 | 295 | case Y_MANEUVER:{ 296 | if (ros::Time::now().toSec() - last_state_time.toSec() > 15.0){ 297 | tau_phi_id = false; 298 | state = YAW_MANEUVER; 299 | tau_psi_id = true; 300 | last_state_time = ros::Time::now(); 301 | while(ros::ok()){ 302 | command_pub.publish(takeoff_pose); 303 | if (target_reached(takeoff_pose)){ 304 | break; 305 | } 306 | ros::spinOnce(); 307 | ros::Duration(rate).sleep(); 308 | } 309 | } 310 | else{ 311 | update_y_maneuver(); 312 | command_pub.publish(y_maneuver_pose); 313 | } 314 | 315 | break; 316 | } 317 | 318 | case YAW_MANEUVER:{ 319 | if (ros::Time::now().toSec() - last_state_time.toSec() > 15.0){ 320 | tau_psi_id = false; 321 | state = LAND; 322 | while(ros::ok()){ 323 | command_pub.publish(takeoff_pose); 324 | if (target_reached(takeoff_pose)){ 325 | break; 326 | } 327 | ros::spinOnce(); 328 | ros::Duration(rate).sleep(); 329 | } 330 | } 331 | else{ 332 | update_yaw_maneuver(); 333 | command_pub.publish(yaw_maneuver_pose); 334 | } 335 | 336 | break; 337 | } 338 | 339 | case LAND:{ 340 | landing_client.call(land_cmd); 341 | if (land_cmd.response.success){ 342 | state = FINISH; 343 | ROS_INFO("Vehicle landed!"); 344 | } 345 | } 346 | 347 | case FINISH:{ 348 | tau_phi = 1 / linear_regression(tau_phi_diff,tau_phi_rate); 349 | tau_theta = 1 / linear_regression(tau_theta_diff,tau_theta_rate); 350 | tau_psi = 1 / linear_regression(tau_psi_diff,tau_psi_rate); 351 | std::cout<<"System identification result:"< 0.25){ 360 | ROS_ERROR("Identified tau_phi out of normal range!"); 361 | } 362 | else if (tau_theta < 0.1 || tau_theta > 0.25){ 363 | ROS_ERROR("Identified tau_theta out of normal range!"); 364 | } 365 | else if (tau_psi < 0.4 || tau_psi > 0.9){ 366 | ROS_ERROR("Identified tau_psi out of normal range!"); 367 | } 368 | std::cout<<"Save the parameters to .yaml file? (y/n)"<>input; 371 | if (input == "y" || input == "\n"){ 372 | write_hover_thrust_to_yaml(backstepping_path); 373 | write_hover_thrust_to_yaml(mpc_path); 374 | write_hover_thrust_to_yaml(slidingmode_path); 375 | write_innerloop_param_to_yaml(mpc_path); 376 | } 377 | std::cout<<"Paramters saved to .yaml files."< 22 | 23 | #ifndef casadi_real 24 | #define casadi_real double 25 | #endif 26 | 27 | #ifndef casadi_int 28 | #define casadi_int int 29 | #endif 30 | 31 | /* Add prefix to internal symbols */ 32 | #define casadi_f0 CASADI_PREFIX(f0) 33 | #define casadi_s0 CASADI_PREFIX(s0) 34 | #define casadi_s1 CASADI_PREFIX(s1) 35 | #define casadi_s2 CASADI_PREFIX(s2) 36 | #define casadi_s3 CASADI_PREFIX(s3) 37 | 38 | /* Symbol visibility in DLLs */ 39 | #ifndef CASADI_SYMBOL_EXPORT 40 | #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) 41 | #if defined(STATIC_LINKED) 42 | #define CASADI_SYMBOL_EXPORT 43 | #else 44 | #define CASADI_SYMBOL_EXPORT __declspec(dllexport) 45 | #endif 46 | #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) 47 | #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) 48 | #else 49 | #define CASADI_SYMBOL_EXPORT 50 | #endif 51 | #endif 52 | 53 | static const casadi_int casadi_s0[12] = {8, 1, 0, 8, 0, 1, 2, 3, 4, 5, 6, 7}; 54 | static const casadi_int casadi_s1[75] = {8, 8, 0, 8, 16, 24, 32, 40, 48, 56, 64, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7}; 55 | static const casadi_int casadi_s2[30] = {8, 3, 0, 8, 16, 24, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7, 0, 1, 2, 3, 4, 5, 6, 7}; 56 | static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2}; 57 | 58 | /* quadrotor_expl_vde_forw:(i0[8],i1[8x8],i2[8x3],i3[3],i4[3])->(o0[8],o1[8x8],o2[8x3]) */ 59 | static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { 60 | casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a4, a5, a6, a7, a8, a9; 61 | a0=arg[0]? arg[0][3] : 0; 62 | if (res[0]!=0) res[0][0]=a0; 63 | a0=arg[0]? arg[0][4] : 0; 64 | if (res[0]!=0) res[0][1]=a0; 65 | a0=arg[0]? arg[0][5] : 0; 66 | if (res[0]!=0) res[0][2]=a0; 67 | a0=arg[0]? arg[0][6] : 0; 68 | a1=cos(a0); 69 | a2=arg[0]? arg[0][7] : 0; 70 | a3=sin(a2); 71 | a4=(a1*a3); 72 | a5=arg[4]? arg[4][2] : 0; 73 | a6=cos(a5); 74 | a4=(a4*a6); 75 | a7=sin(a0); 76 | a8=sin(a5); 77 | a7=(a7*a8); 78 | a4=(a4+a7); 79 | a7=arg[3]? arg[3][0] : 0; 80 | a9=(a4*a7); 81 | if (res[0]!=0) res[0][3]=a9; 82 | a9=cos(a0); 83 | a10=sin(a2); 84 | a11=(a9*a10); 85 | a12=sin(a5); 86 | a11=(a11*a12); 87 | a13=sin(a0); 88 | a5=cos(a5); 89 | a13=(a13*a5); 90 | a11=(a11-a13); 91 | a13=(a11*a7); 92 | if (res[0]!=0) res[0][4]=a13; 93 | a13=-9.8066499999999994e+00; 94 | a14=cos(a2); 95 | a15=cos(a0); 96 | a16=(a14*a15); 97 | a17=(a16*a7); 98 | a13=(a13+a17); 99 | if (res[0]!=0) res[0][5]=a13; 100 | a13=arg[3]? arg[3][1] : 0; 101 | a13=(a13-a0); 102 | a17=arg[4]? arg[4][0] : 0; 103 | a13=(a13/a17); 104 | if (res[0]!=0) res[0][6]=a13; 105 | a13=arg[3]? arg[3][2] : 0; 106 | a13=(a13-a2); 107 | a18=arg[4]? arg[4][1] : 0; 108 | a13=(a13/a18); 109 | if (res[0]!=0) res[0][7]=a13; 110 | a13=arg[1]? arg[1][3] : 0; 111 | if (res[1]!=0) res[1][0]=a13; 112 | a13=arg[1]? arg[1][4] : 0; 113 | if (res[1]!=0) res[1][1]=a13; 114 | a13=arg[1]? arg[1][5] : 0; 115 | if (res[1]!=0) res[1][2]=a13; 116 | a13=cos(a2); 117 | a19=arg[1]? arg[1][7] : 0; 118 | a20=(a13*a19); 119 | a20=(a1*a20); 120 | a21=sin(a0); 121 | a22=arg[1]? arg[1][6] : 0; 122 | a23=(a21*a22); 123 | a23=(a3*a23); 124 | a20=(a20-a23); 125 | a20=(a6*a20); 126 | a23=cos(a0); 127 | a24=(a23*a22); 128 | a24=(a8*a24); 129 | a20=(a20+a24); 130 | a20=(a7*a20); 131 | if (res[1]!=0) res[1][3]=a20; 132 | a20=cos(a2); 133 | a24=(a20*a19); 134 | a24=(a9*a24); 135 | a25=sin(a0); 136 | a26=(a25*a22); 137 | a26=(a10*a26); 138 | a24=(a24-a26); 139 | a24=(a12*a24); 140 | a26=cos(a0); 141 | a27=(a26*a22); 142 | a27=(a5*a27); 143 | a24=(a24-a27); 144 | a24=(a7*a24); 145 | if (res[1]!=0) res[1][4]=a24; 146 | a24=sin(a2); 147 | a27=(a24*a19); 148 | a27=(a15*a27); 149 | a28=sin(a0); 150 | a29=(a28*a22); 151 | a29=(a14*a29); 152 | a27=(a27+a29); 153 | a27=(a7*a27); 154 | a27=(-a27); 155 | if (res[1]!=0) res[1][5]=a27; 156 | a22=(a22/a17); 157 | a22=(-a22); 158 | if (res[1]!=0) res[1][6]=a22; 159 | a19=(a19/a18); 160 | a19=(-a19); 161 | if (res[1]!=0) res[1][7]=a19; 162 | a19=arg[1]? arg[1][11] : 0; 163 | if (res[1]!=0) res[1][8]=a19; 164 | a19=arg[1]? arg[1][12] : 0; 165 | if (res[1]!=0) res[1][9]=a19; 166 | a19=arg[1]? arg[1][13] : 0; 167 | if (res[1]!=0) res[1][10]=a19; 168 | a19=arg[1]? arg[1][15] : 0; 169 | a22=(a13*a19); 170 | a22=(a1*a22); 171 | a27=arg[1]? arg[1][14] : 0; 172 | a29=(a21*a27); 173 | a29=(a3*a29); 174 | a22=(a22-a29); 175 | a22=(a6*a22); 176 | a29=(a23*a27); 177 | a29=(a8*a29); 178 | a22=(a22+a29); 179 | a22=(a7*a22); 180 | if (res[1]!=0) res[1][11]=a22; 181 | a22=(a20*a19); 182 | a22=(a9*a22); 183 | a29=(a25*a27); 184 | a29=(a10*a29); 185 | a22=(a22-a29); 186 | a22=(a12*a22); 187 | a29=(a26*a27); 188 | a29=(a5*a29); 189 | a22=(a22-a29); 190 | a22=(a7*a22); 191 | if (res[1]!=0) res[1][12]=a22; 192 | a22=(a24*a19); 193 | a22=(a15*a22); 194 | a29=(a28*a27); 195 | a29=(a14*a29); 196 | a22=(a22+a29); 197 | a22=(a7*a22); 198 | a22=(-a22); 199 | if (res[1]!=0) res[1][13]=a22; 200 | a27=(a27/a17); 201 | a27=(-a27); 202 | if (res[1]!=0) res[1][14]=a27; 203 | a19=(a19/a18); 204 | a19=(-a19); 205 | if (res[1]!=0) res[1][15]=a19; 206 | a19=arg[1]? arg[1][19] : 0; 207 | if (res[1]!=0) res[1][16]=a19; 208 | a19=arg[1]? arg[1][20] : 0; 209 | if (res[1]!=0) res[1][17]=a19; 210 | a19=arg[1]? arg[1][21] : 0; 211 | if (res[1]!=0) res[1][18]=a19; 212 | a19=arg[1]? arg[1][23] : 0; 213 | a27=(a13*a19); 214 | a27=(a1*a27); 215 | a22=arg[1]? arg[1][22] : 0; 216 | a29=(a21*a22); 217 | a29=(a3*a29); 218 | a27=(a27-a29); 219 | a27=(a6*a27); 220 | a29=(a23*a22); 221 | a29=(a8*a29); 222 | a27=(a27+a29); 223 | a27=(a7*a27); 224 | if (res[1]!=0) res[1][19]=a27; 225 | a27=(a20*a19); 226 | a27=(a9*a27); 227 | a29=(a25*a22); 228 | a29=(a10*a29); 229 | a27=(a27-a29); 230 | a27=(a12*a27); 231 | a29=(a26*a22); 232 | a29=(a5*a29); 233 | a27=(a27-a29); 234 | a27=(a7*a27); 235 | if (res[1]!=0) res[1][20]=a27; 236 | a27=(a24*a19); 237 | a27=(a15*a27); 238 | a29=(a28*a22); 239 | a29=(a14*a29); 240 | a27=(a27+a29); 241 | a27=(a7*a27); 242 | a27=(-a27); 243 | if (res[1]!=0) res[1][21]=a27; 244 | a22=(a22/a17); 245 | a22=(-a22); 246 | if (res[1]!=0) res[1][22]=a22; 247 | a19=(a19/a18); 248 | a19=(-a19); 249 | if (res[1]!=0) res[1][23]=a19; 250 | a19=arg[1]? arg[1][27] : 0; 251 | if (res[1]!=0) res[1][24]=a19; 252 | a19=arg[1]? arg[1][28] : 0; 253 | if (res[1]!=0) res[1][25]=a19; 254 | a19=arg[1]? arg[1][29] : 0; 255 | if (res[1]!=0) res[1][26]=a19; 256 | a19=arg[1]? arg[1][31] : 0; 257 | a22=(a13*a19); 258 | a22=(a1*a22); 259 | a27=arg[1]? arg[1][30] : 0; 260 | a29=(a21*a27); 261 | a29=(a3*a29); 262 | a22=(a22-a29); 263 | a22=(a6*a22); 264 | a29=(a23*a27); 265 | a29=(a8*a29); 266 | a22=(a22+a29); 267 | a22=(a7*a22); 268 | if (res[1]!=0) res[1][27]=a22; 269 | a22=(a20*a19); 270 | a22=(a9*a22); 271 | a29=(a25*a27); 272 | a29=(a10*a29); 273 | a22=(a22-a29); 274 | a22=(a12*a22); 275 | a29=(a26*a27); 276 | a29=(a5*a29); 277 | a22=(a22-a29); 278 | a22=(a7*a22); 279 | if (res[1]!=0) res[1][28]=a22; 280 | a22=(a24*a19); 281 | a22=(a15*a22); 282 | a29=(a28*a27); 283 | a29=(a14*a29); 284 | a22=(a22+a29); 285 | a22=(a7*a22); 286 | a22=(-a22); 287 | if (res[1]!=0) res[1][29]=a22; 288 | a27=(a27/a17); 289 | a27=(-a27); 290 | if (res[1]!=0) res[1][30]=a27; 291 | a19=(a19/a18); 292 | a19=(-a19); 293 | if (res[1]!=0) res[1][31]=a19; 294 | a19=arg[1]? arg[1][35] : 0; 295 | if (res[1]!=0) res[1][32]=a19; 296 | a19=arg[1]? arg[1][36] : 0; 297 | if (res[1]!=0) res[1][33]=a19; 298 | a19=arg[1]? arg[1][37] : 0; 299 | if (res[1]!=0) res[1][34]=a19; 300 | a19=arg[1]? arg[1][39] : 0; 301 | a27=(a13*a19); 302 | a27=(a1*a27); 303 | a22=arg[1]? arg[1][38] : 0; 304 | a29=(a21*a22); 305 | a29=(a3*a29); 306 | a27=(a27-a29); 307 | a27=(a6*a27); 308 | a29=(a23*a22); 309 | a29=(a8*a29); 310 | a27=(a27+a29); 311 | a27=(a7*a27); 312 | if (res[1]!=0) res[1][35]=a27; 313 | a27=(a20*a19); 314 | a27=(a9*a27); 315 | a29=(a25*a22); 316 | a29=(a10*a29); 317 | a27=(a27-a29); 318 | a27=(a12*a27); 319 | a29=(a26*a22); 320 | a29=(a5*a29); 321 | a27=(a27-a29); 322 | a27=(a7*a27); 323 | if (res[1]!=0) res[1][36]=a27; 324 | a27=(a24*a19); 325 | a27=(a15*a27); 326 | a29=(a28*a22); 327 | a29=(a14*a29); 328 | a27=(a27+a29); 329 | a27=(a7*a27); 330 | a27=(-a27); 331 | if (res[1]!=0) res[1][37]=a27; 332 | a22=(a22/a17); 333 | a22=(-a22); 334 | if (res[1]!=0) res[1][38]=a22; 335 | a19=(a19/a18); 336 | a19=(-a19); 337 | if (res[1]!=0) res[1][39]=a19; 338 | a19=arg[1]? arg[1][43] : 0; 339 | if (res[1]!=0) res[1][40]=a19; 340 | a19=arg[1]? arg[1][44] : 0; 341 | if (res[1]!=0) res[1][41]=a19; 342 | a19=arg[1]? arg[1][45] : 0; 343 | if (res[1]!=0) res[1][42]=a19; 344 | a19=arg[1]? arg[1][47] : 0; 345 | a22=(a13*a19); 346 | a22=(a1*a22); 347 | a27=arg[1]? arg[1][46] : 0; 348 | a29=(a21*a27); 349 | a29=(a3*a29); 350 | a22=(a22-a29); 351 | a22=(a6*a22); 352 | a29=(a23*a27); 353 | a29=(a8*a29); 354 | a22=(a22+a29); 355 | a22=(a7*a22); 356 | if (res[1]!=0) res[1][43]=a22; 357 | a22=(a20*a19); 358 | a22=(a9*a22); 359 | a29=(a25*a27); 360 | a29=(a10*a29); 361 | a22=(a22-a29); 362 | a22=(a12*a22); 363 | a29=(a26*a27); 364 | a29=(a5*a29); 365 | a22=(a22-a29); 366 | a22=(a7*a22); 367 | if (res[1]!=0) res[1][44]=a22; 368 | a22=(a24*a19); 369 | a22=(a15*a22); 370 | a29=(a28*a27); 371 | a29=(a14*a29); 372 | a22=(a22+a29); 373 | a22=(a7*a22); 374 | a22=(-a22); 375 | if (res[1]!=0) res[1][45]=a22; 376 | a27=(a27/a17); 377 | a27=(-a27); 378 | if (res[1]!=0) res[1][46]=a27; 379 | a19=(a19/a18); 380 | a19=(-a19); 381 | if (res[1]!=0) res[1][47]=a19; 382 | a19=arg[1]? arg[1][51] : 0; 383 | if (res[1]!=0) res[1][48]=a19; 384 | a19=arg[1]? arg[1][52] : 0; 385 | if (res[1]!=0) res[1][49]=a19; 386 | a19=arg[1]? arg[1][53] : 0; 387 | if (res[1]!=0) res[1][50]=a19; 388 | a19=arg[1]? arg[1][55] : 0; 389 | a27=(a13*a19); 390 | a27=(a1*a27); 391 | a22=arg[1]? arg[1][54] : 0; 392 | a29=(a21*a22); 393 | a29=(a3*a29); 394 | a27=(a27-a29); 395 | a27=(a6*a27); 396 | a29=(a23*a22); 397 | a29=(a8*a29); 398 | a27=(a27+a29); 399 | a27=(a7*a27); 400 | if (res[1]!=0) res[1][51]=a27; 401 | a27=(a20*a19); 402 | a27=(a9*a27); 403 | a29=(a25*a22); 404 | a29=(a10*a29); 405 | a27=(a27-a29); 406 | a27=(a12*a27); 407 | a29=(a26*a22); 408 | a29=(a5*a29); 409 | a27=(a27-a29); 410 | a27=(a7*a27); 411 | if (res[1]!=0) res[1][52]=a27; 412 | a27=(a24*a19); 413 | a27=(a15*a27); 414 | a29=(a28*a22); 415 | a29=(a14*a29); 416 | a27=(a27+a29); 417 | a27=(a7*a27); 418 | a27=(-a27); 419 | if (res[1]!=0) res[1][53]=a27; 420 | a22=(a22/a17); 421 | a22=(-a22); 422 | if (res[1]!=0) res[1][54]=a22; 423 | a19=(a19/a18); 424 | a19=(-a19); 425 | if (res[1]!=0) res[1][55]=a19; 426 | a19=arg[1]? arg[1][59] : 0; 427 | if (res[1]!=0) res[1][56]=a19; 428 | a19=arg[1]? arg[1][60] : 0; 429 | if (res[1]!=0) res[1][57]=a19; 430 | a19=arg[1]? arg[1][61] : 0; 431 | if (res[1]!=0) res[1][58]=a19; 432 | a19=arg[1]? arg[1][63] : 0; 433 | a13=(a13*a19); 434 | a13=(a1*a13); 435 | a22=arg[1]? arg[1][62] : 0; 436 | a21=(a21*a22); 437 | a21=(a3*a21); 438 | a13=(a13-a21); 439 | a13=(a6*a13); 440 | a23=(a23*a22); 441 | a23=(a8*a23); 442 | a13=(a13+a23); 443 | a13=(a7*a13); 444 | if (res[1]!=0) res[1][59]=a13; 445 | a20=(a20*a19); 446 | a20=(a9*a20); 447 | a25=(a25*a22); 448 | a25=(a10*a25); 449 | a20=(a20-a25); 450 | a20=(a12*a20); 451 | a26=(a26*a22); 452 | a26=(a5*a26); 453 | a20=(a20-a26); 454 | a20=(a7*a20); 455 | if (res[1]!=0) res[1][60]=a20; 456 | a24=(a24*a19); 457 | a24=(a15*a24); 458 | a28=(a28*a22); 459 | a28=(a14*a28); 460 | a24=(a24+a28); 461 | a24=(a7*a24); 462 | a24=(-a24); 463 | if (res[1]!=0) res[1][61]=a24; 464 | a22=(a22/a17); 465 | a22=(-a22); 466 | if (res[1]!=0) res[1][62]=a22; 467 | a19=(a19/a18); 468 | a19=(-a19); 469 | if (res[1]!=0) res[1][63]=a19; 470 | a19=arg[2]? arg[2][3] : 0; 471 | if (res[2]!=0) res[2][0]=a19; 472 | a19=arg[2]? arg[2][4] : 0; 473 | if (res[2]!=0) res[2][1]=a19; 474 | a19=arg[2]? arg[2][5] : 0; 475 | if (res[2]!=0) res[2][2]=a19; 476 | a19=cos(a2); 477 | a22=arg[2]? arg[2][7] : 0; 478 | a24=(a19*a22); 479 | a24=(a1*a24); 480 | a28=sin(a0); 481 | a20=arg[2]? arg[2][6] : 0; 482 | a26=(a28*a20); 483 | a26=(a3*a26); 484 | a24=(a24-a26); 485 | a24=(a6*a24); 486 | a26=cos(a0); 487 | a25=(a26*a20); 488 | a25=(a8*a25); 489 | a24=(a24+a25); 490 | a24=(a7*a24); 491 | a4=(a4+a24); 492 | if (res[2]!=0) res[2][3]=a4; 493 | a4=cos(a2); 494 | a24=(a4*a22); 495 | a24=(a9*a24); 496 | a25=sin(a0); 497 | a13=(a25*a20); 498 | a13=(a10*a13); 499 | a24=(a24-a13); 500 | a24=(a12*a24); 501 | a13=cos(a0); 502 | a23=(a13*a20); 503 | a23=(a5*a23); 504 | a24=(a24-a23); 505 | a24=(a7*a24); 506 | a11=(a11+a24); 507 | if (res[2]!=0) res[2][4]=a11; 508 | a2=sin(a2); 509 | a11=(a2*a22); 510 | a11=(a15*a11); 511 | a0=sin(a0); 512 | a24=(a0*a20); 513 | a24=(a14*a24); 514 | a11=(a11+a24); 515 | a11=(a7*a11); 516 | a16=(a16-a11); 517 | if (res[2]!=0) res[2][5]=a16; 518 | a20=(a20/a17); 519 | a20=(-a20); 520 | if (res[2]!=0) res[2][6]=a20; 521 | a22=(a22/a18); 522 | a22=(-a22); 523 | if (res[2]!=0) res[2][7]=a22; 524 | a22=arg[2]? arg[2][11] : 0; 525 | if (res[2]!=0) res[2][8]=a22; 526 | a22=arg[2]? arg[2][12] : 0; 527 | if (res[2]!=0) res[2][9]=a22; 528 | a22=arg[2]? arg[2][13] : 0; 529 | if (res[2]!=0) res[2][10]=a22; 530 | a22=arg[2]? arg[2][15] : 0; 531 | a20=(a19*a22); 532 | a20=(a1*a20); 533 | a16=arg[2]? arg[2][14] : 0; 534 | a11=(a28*a16); 535 | a11=(a3*a11); 536 | a20=(a20-a11); 537 | a20=(a6*a20); 538 | a11=(a26*a16); 539 | a11=(a8*a11); 540 | a20=(a20+a11); 541 | a20=(a7*a20); 542 | if (res[2]!=0) res[2][11]=a20; 543 | a20=(a4*a22); 544 | a20=(a9*a20); 545 | a11=(a25*a16); 546 | a11=(a10*a11); 547 | a20=(a20-a11); 548 | a20=(a12*a20); 549 | a11=(a13*a16); 550 | a11=(a5*a11); 551 | a20=(a20-a11); 552 | a20=(a7*a20); 553 | if (res[2]!=0) res[2][12]=a20; 554 | a20=(a2*a22); 555 | a20=(a15*a20); 556 | a11=(a0*a16); 557 | a11=(a14*a11); 558 | a20=(a20+a11); 559 | a20=(a7*a20); 560 | a20=(-a20); 561 | if (res[2]!=0) res[2][13]=a20; 562 | a20=(1./a17); 563 | a16=(a16/a17); 564 | a20=(a20-a16); 565 | if (res[2]!=0) res[2][14]=a20; 566 | a22=(a22/a18); 567 | a22=(-a22); 568 | if (res[2]!=0) res[2][15]=a22; 569 | a22=arg[2]? arg[2][19] : 0; 570 | if (res[2]!=0) res[2][16]=a22; 571 | a22=arg[2]? arg[2][20] : 0; 572 | if (res[2]!=0) res[2][17]=a22; 573 | a22=arg[2]? arg[2][21] : 0; 574 | if (res[2]!=0) res[2][18]=a22; 575 | a22=arg[2]? arg[2][23] : 0; 576 | a19=(a19*a22); 577 | a1=(a1*a19); 578 | a19=arg[2]? arg[2][22] : 0; 579 | a28=(a28*a19); 580 | a3=(a3*a28); 581 | a1=(a1-a3); 582 | a6=(a6*a1); 583 | a26=(a26*a19); 584 | a8=(a8*a26); 585 | a6=(a6+a8); 586 | a6=(a7*a6); 587 | if (res[2]!=0) res[2][19]=a6; 588 | a4=(a4*a22); 589 | a9=(a9*a4); 590 | a25=(a25*a19); 591 | a10=(a10*a25); 592 | a9=(a9-a10); 593 | a12=(a12*a9); 594 | a13=(a13*a19); 595 | a5=(a5*a13); 596 | a12=(a12-a5); 597 | a12=(a7*a12); 598 | if (res[2]!=0) res[2][20]=a12; 599 | a2=(a2*a22); 600 | a15=(a15*a2); 601 | a0=(a0*a19); 602 | a14=(a14*a0); 603 | a15=(a15+a14); 604 | a7=(a7*a15); 605 | a7=(-a7); 606 | if (res[2]!=0) res[2][21]=a7; 607 | a19=(a19/a17); 608 | a19=(-a19); 609 | if (res[2]!=0) res[2][22]=a19; 610 | a19=(1./a18); 611 | a22=(a22/a18); 612 | a19=(a19-a22); 613 | if (res[2]!=0) res[2][23]=a19; 614 | return 0; 615 | } 616 | 617 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ 618 | return casadi_f0(arg, res, iw, w, mem); 619 | } 620 | 621 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_forw_alloc_mem(void) { 622 | return 0; 623 | } 624 | 625 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_forw_init_mem(int mem) { 626 | return 0; 627 | } 628 | 629 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_forw_free_mem(int mem) { 630 | } 631 | 632 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_forw_checkout(void) { 633 | return 0; 634 | } 635 | 636 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_forw_release(int mem) { 637 | } 638 | 639 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_forw_incref(void) { 640 | } 641 | 642 | CASADI_SYMBOL_EXPORT void quadrotor_expl_vde_forw_decref(void) { 643 | } 644 | 645 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_vde_forw_n_in(void) { return 5;} 646 | 647 | CASADI_SYMBOL_EXPORT casadi_int quadrotor_expl_vde_forw_n_out(void) { return 3;} 648 | 649 | CASADI_SYMBOL_EXPORT casadi_real quadrotor_expl_vde_forw_default_in(casadi_int i) { 650 | switch (i) { 651 | default: return 0; 652 | } 653 | } 654 | 655 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_vde_forw_name_in(casadi_int i) { 656 | switch (i) { 657 | case 0: return "i0"; 658 | case 1: return "i1"; 659 | case 2: return "i2"; 660 | case 3: return "i3"; 661 | case 4: return "i4"; 662 | default: return 0; 663 | } 664 | } 665 | 666 | CASADI_SYMBOL_EXPORT const char* quadrotor_expl_vde_forw_name_out(casadi_int i) { 667 | switch (i) { 668 | case 0: return "o0"; 669 | case 1: return "o1"; 670 | case 2: return "o2"; 671 | default: return 0; 672 | } 673 | } 674 | 675 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_vde_forw_sparsity_in(casadi_int i) { 676 | switch (i) { 677 | case 0: return casadi_s0; 678 | case 1: return casadi_s1; 679 | case 2: return casadi_s2; 680 | case 3: return casadi_s3; 681 | case 4: return casadi_s3; 682 | default: return 0; 683 | } 684 | } 685 | 686 | CASADI_SYMBOL_EXPORT const casadi_int* quadrotor_expl_vde_forw_sparsity_out(casadi_int i) { 687 | switch (i) { 688 | case 0: return casadi_s0; 689 | case 1: return casadi_s1; 690 | case 2: return casadi_s2; 691 | default: return 0; 692 | } 693 | } 694 | 695 | CASADI_SYMBOL_EXPORT int quadrotor_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { 696 | if (sz_arg) *sz_arg = 5; 697 | if (sz_res) *sz_res = 3; 698 | if (sz_iw) *sz_iw = 0; 699 | if (sz_w) *sz_w = 0; 700 | return 0; 701 | } 702 | 703 | 704 | #ifdef __cplusplus 705 | } /* extern "C" */ 706 | #endif 707 | --------------------------------------------------------------------------------