├── .gitignore ├── launch ├── plot.launch ├── circle.launch ├── lemniscate.launch ├── points.launch └── px4_gazebo.launch ├── traj ├── lemniscate.py ├── circle.py └── points.py ├── CMakeLists.txt ├── scripts ├── plot.py ├── quadrotor.py └── generate_c_code.py ├── package.xml ├── README.md └── src └── qr_mpc_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | scripts/c_generated_code 2 | scripts/build 3 | scripts/acados_ocp_nlp.json 4 | scripts/__pycache__ 5 | scripts/acados_ocp.json 6 | .vscode 7 | .gitignore -------------------------------------------------------------------------------- /launch/plot.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | -------------------------------------------------------------------------------- /launch/circle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/lemniscate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/points.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /traj/lemniscate.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------- 2 | #Generate reference trajectory for NMPC 3 | #-------------------------------------- 4 | 5 | import numpy as np 6 | 7 | # Parameters 8 | sample_time = 0.025 #seconds 9 | duration = 60; #seconds 10 | hover_thrust = 0.26 11 | amp = 2 12 | frq = 2 13 | 14 | x0 = 0 15 | y0 = 0 16 | z0 = 1 17 | 18 | # Trajectory 19 | traj = np.zeros((int(duration/sample_time+1),11)) #x y z u v w phi theta thrust phi_cmd theta_cmd 20 | t = np.arange(0,duration,sample_time) 21 | t = np.append(t, duration) 22 | 23 | traj[:,0] = amp*np.cos(t*frq)+x0 24 | traj[:,1] = amp*np.sin(t*frq)*np.cos(t*frq)+y0 25 | traj[:,2] = z0 26 | traj[:,3] = -amp*frq*np.sin(t*frq) 27 | traj[:,4] = amp*frq*np.cos(t*2*frq) 28 | traj[:,5] = 0 29 | traj[:,6] = 0 30 | traj[:,7] = 0 31 | traj[:,8] = hover_thrust 32 | traj[:,9] = 0 33 | traj[:,10] = 0 34 | 35 | # write to txt 36 | np.savetxt('lemniscate.txt',traj,fmt='%f') -------------------------------------------------------------------------------- /traj/circle.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------- 2 | #Generate reference trajectory for NMPC 3 | #-------------------------------------- 4 | 5 | import numpy as np 6 | import math 7 | 8 | # Parameters 9 | sample_time = 0.025 # seconds 10 | duration = 60 # seconds 11 | hover_thrust = 0.56 12 | r = 1.5 13 | v = 2 14 | 15 | x0 = 0.5 16 | y0 = 0.5 17 | z0 = 1 18 | 19 | # trajectory 20 | traj = np.zeros((int(duration/sample_time+1),13)) #x y z u v w phi theta psi thrust phi_cmd theta_cmd psi_cmd 21 | t = np.arange(0,duration,sample_time) 22 | t = np.append(t, duration) 23 | 24 | traj[:,0] = -r*np.cos(t*v/r)+x0 25 | traj[:,1] = -r*np.sin(t*v/r)+y0 26 | traj[:,2] = z0 27 | traj[:,3] = v*np.sin(t*v/r) 28 | traj[:,4] = -v*np.cos(t*v/r) 29 | traj[:,5] = 0 30 | traj[:,6] = 0 31 | traj[:,7] = 0 32 | traj[:,8] = hover_thrust 33 | traj[:,9] = 0 34 | traj[:,10] = 0 35 | 36 | 37 | 38 | #print(np.size(traj)) 39 | 40 | 41 | # write to txt 42 | np.savetxt('circle.txt',traj,fmt='%f') 43 | -------------------------------------------------------------------------------- /launch/px4_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 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(qr_mpc) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 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 | ) 13 | 14 | find_package( 15 | Eigen3 REQUIRED 16 | ) 17 | 18 | set(acados_include "/root/acados/include") 19 | set(acados_lib "/root/acados/lib") 20 | set(quadrotor_model ${PROJECT_SOURCE_DIR}/scripts/c_generated_code) 21 | 22 | catkin_package( 23 | # INCLUDE_DIRS include 24 | # LIBRARIES qr_mpc 25 | # CATKIN_DEPENDS roscpp 26 | # DEPENDS system_lib 27 | ) 28 | 29 | include_directories( 30 | # include 31 | ${catkin_INCLUDE_DIRS} 32 | ${quadrotor_model} 33 | ${acados_include} 34 | ${acados_include}/blasfeo/include/ 35 | ${acados_include}/hpipm/include/ 36 | ${acados_include}/acados/ 37 | ${acados_include}/qpOASES_e/ 38 | ) 39 | 40 | link_directories(${quadrotor_model}) 41 | 42 | add_executable(qr_mpc_node src/qr_mpc_node.cpp) 43 | target_link_libraries(qr_mpc_node 44 | ${quadrotor_model}/libacados_ocp_solver_quadrotor.so 45 | ${acados_lib}/libacados.so 46 | ${catkin_LIBRARIES} 47 | ) 48 | -------------------------------------------------------------------------------- /traj/points.py: -------------------------------------------------------------------------------- 1 | #-------------------------------------- 2 | #Generate reference trajectory for NMPC 3 | #-------------------------------------- 4 | 5 | import numpy as np 6 | import numpy.matlib 7 | # Parameters 8 | sample_time = 0.025 #seconds 9 | hover_thrust = 0.26 10 | cycles = 5 11 | step_interval = 5 12 | 13 | points_matrix = np.array([[-0.5,-0.5,1],[1.5,-0.5,1],[1.5,1.5,1],[-0.5,1.5,1]]) 14 | 15 | # Trajectory 16 | duration = cycles*np.size(points_matrix,0)*step_interval 17 | 18 | traj = np.zeros((int(duration/sample_time+1),11)) #x y z u v w phi theta thrust phi_cmd theta_cmd 19 | 20 | t = np.arange(0,duration,sample_time) 21 | t = np.append(t, duration) 22 | 23 | traj[:,3] = 0 24 | traj[:,4] = 0 25 | traj[:,5] = 0 26 | traj[:,6] = 0 27 | traj[:,7] = 0 28 | traj[:,8] = hover_thrust 29 | traj[:,9] = 0 30 | traj[:,10] = 0 31 | 32 | for i in range(1,cycles+1): 33 | for j in range(1,np.size(points_matrix,0)+1): 34 | traj_start = (i-1)*np.size(points_matrix,0)*step_interval+(j-1)*step_interval 35 | traj_end = (i-1)*np.size(points_matrix,0)*step_interval+j*step_interval 36 | traj[int(traj_start/sample_time):int(traj_end/sample_time),0:3] = np.tile(points_matrix[j-1,:],(int(step_interval/sample_time),1)) 37 | 38 | traj[-1,0:3] = traj[-2,0:3] 39 | 40 | # write to txt 41 | np.savetxt('points.txt',traj,fmt='%f') -------------------------------------------------------------------------------- /scripts/plot.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from nav_msgs.msg import Odometry # replace with the actual message type of pose_gt and mpc_reference topics 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import matplotlib.pyplot as plt 5 | 6 | real_x, real_y = [], [] 7 | ref_x, ref_y = [], [] 8 | gt_start = False 9 | ref_start = False 10 | def gt_callback(data): 11 | global real_x, real_y 12 | global gt_start 13 | gt_start = True 14 | real_x.append(data.pose.pose.position.x) 15 | real_y.append(data.pose.pose.position.y) 16 | 17 | 18 | def ref_callback(data): 19 | global ref_start 20 | ref_start = True 21 | global ref_x, ref_y 22 | ref_x.append(data.pose.pose.position.x) 23 | ref_y.append(data.pose.pose.position.y) 24 | 25 | 26 | rospy.init_node('plotter', anonymous=True) 27 | rospy.Subscriber('/bluerov2/pose_gt', Odometry, gt_callback) 28 | rospy.Subscriber('/bluerov2/mpc/reference', Odometry, ref_callback) 29 | 30 | fig, ax = plt.subplots() 31 | 32 | while not rospy.is_shutdown(): 33 | if gt_start==True and ref_start==True: 34 | ax.clear() 35 | ax.set_xlabel('X') 36 | ax.set_ylabel('Y') 37 | ax.set_xlim([10, 25]) 38 | ax.set_ylim([0, 15]) 39 | ax.plot(real_x[-20:], real_y[-20:], label='real', color='blue') 40 | ax.plot(ref_x[-20:], ref_y[-20:], label='reference', color='orange') 41 | ax.scatter(real_x[-1], real_y[-1], color='blue', marker='o', label='current') 42 | ax.scatter(ref_x[-1], ref_y[-1], color='orange', marker='o', label='reference_current') 43 | ax.legend() 44 | plt.draw() 45 | plt.pause(0.05) 46 | 47 | plt.show() -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | qr_mpc 4 | 0.0.0 5 | The qr_mpc package 6 | 7 | 8 | 9 | 10 | rocky 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | roscpp 53 | roscpp 54 | roscpp 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # qr_mpc 2 | This work implements a NMPC controller for a quadcopter with [ROS](https://www.ros.org/). 3 | 4 | ## Prerequisites 5 | * Python 3.7 6 | * ROS ([ROS noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) recommended) 7 | * [QGroundControl](http://qgroundcontrol.com/) 8 | * [MAVROS](http://wiki.ros.org/mavros) 9 | * [Acados](https://docs.acados.org/installation/index.html) 10 | 11 | ## Getting started 12 | Install python 3.7 13 | ``` 14 | sudo add-apt-repository ppa:deadsnakes/ppa 15 | sudo apt update 16 | sudo apt install python3.7 17 | ``` 18 | Install python dependencies 19 | ``` 20 | python3 -m pip install pip 21 | sudo pip3 install numpy matplotlib scipy future-fstrings casadi>=3.5.1 setuptools 22 | sudo apt-get install python3.7-tk 23 | ``` 24 | Install Acados 25 | ``` 26 | git clone https://github.com/acados/acados.git 27 | cd acados 28 | git checkout 568e46c 29 | git submodule update --recursive --init 30 | mkdir -p build 31 | cd build 32 | cmake -DACADOS_WITH_QPOASES=ON -DACADOS_WITH_OSQP=OFF/ON -DACADOS_INSTALL_DIR= .. 33 | sudo make install -j4 34 | ``` 35 | Create a catkin workspace and clone this repository to catkin src folder (ex. ~/catkin_ws/src) 36 | ``` 37 | mkdir -p ~/catkin_ws/src 38 | cd ~/catkin_ws/ 39 | catkin_make 40 | cd src 41 | git clone https://github.com/HKPolyU-UAV/qr_mpc.git 42 | ``` 43 | Install acados_template python packages 44 | ``` 45 | pip install -e /interfaces/acados_template 46 | ``` 47 | Add the path to the compiled shared libraries (Hint: you can add these lines to your ```.bashrc``` by ```sudo gedit ~/.bashrc```) 48 | ``` 49 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/lib" 50 | export ACADOS_SOURCE_DIR="" 51 | ``` 52 | Modify path (18th and 19th lines) in CMakeLists 53 | ``` 54 | cd ~/catkin_ws/src/qr_mpc/ 55 | gedit CMakeLists.txt 56 | ``` 57 | Generate C code for NMPC controller 58 | ``` 59 | cd scripts 60 | python3 generate_c_code.py 61 | ``` 62 | Compile 63 | ``` 64 | cd ~/catkin_ws/ 65 | catkin_make 66 | ``` 67 | Download and install the PX4 (1.11.0) 68 | ``` 69 | cd ~ 70 | git clone https://github.com/PX4/PX4-Autopilot.git 71 | cd PX4-Autopilot/ 72 | git checkout 71db090 73 | git submodule sync --recursive 74 | git submodule update --init --recursive 75 | bash ./Tools/setup/ubuntu.sh 76 | sudo apt upgrade libignition-math4 #(libignition-math2 for melodic) 77 | ``` 78 | 79 | ## Running 80 | Start PX4 SITL 81 | ``` 82 | cd ~/PX4-Autopilot/ 83 | make px4_sitl_default gazebo 84 | ``` 85 | Run MAVROS 86 | ``` 87 | cd ~/catkin_ws/ 88 | roslaunch qr_mpc px4_gazebo.launch 89 | ``` 90 | Open QGroundControl to takoff 91 | 92 | Run NMPC controller (there are three preset trajectories in this repo: circle, lemniscate and waypoints, circle is used as example) 93 | ``` 94 | roslaunch qr_mpc circle.launch 95 | ``` 96 | Set flight mode to "offboard" in QGroundControl 97 | -------------------------------------------------------------------------------- /scripts/quadrotor.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosModel 2 | from casadi import SX, vertcat, sin, cos 3 | 4 | def export_quadrotor_model() -> AcadosModel: 5 | 6 | model_name = 'quadrotor' 7 | 8 | # system parameters 9 | g = 9.81 # gravity constant [m/s^2] 10 | hover_thrust = 0.56 11 | tau_phi = 0.1667 # Inner-loop controller time constants 12 | tau_theta = 0.1667 13 | tau_psi = 0.1667 14 | 15 | # states 16 | x = SX.sym('x') # earth position x 17 | y = SX.sym('y') # earth position y 18 | z = SX.sym('z') # earth position z 19 | u = SX.sym('u') # earth velocity x 20 | v = SX.sym('v') # earth velocity y 21 | w = SX.sym('w') # earth velocity z 22 | phi = SX.sym('phi') # roll angle phi 23 | theta = SX.sym('theta') # pitch angle 24 | sym_x = vertcat(x,y,z,u,v,w,phi,theta) 25 | 26 | # controls 27 | thrust = SX.sym('thrust') # thrust command 28 | phi_cmd = SX.sym('phi_cmd') # roll angle command 29 | theta_cmd = SX.sym('theta_cmd') # pitch angle command 30 | sym_u = vertcat(thrust,phi_cmd,theta_cmd) 31 | 32 | # xdot for f_impl 33 | x_dot = SX.sym('x_dot') 34 | y_dot = SX.sym('y_dot') 35 | z_dot = SX.sym('z_dot') 36 | u_dot = SX.sym('u_dot') 37 | v_dot = SX.sym('v_dot') 38 | w_dot = SX.sym('w_dot') 39 | phi_dot = SX.sym('phi_dot') 40 | theta_dot = SX.sym('theta_dot') 41 | sym_xdot = vertcat(x_dot,y_dot,z_dot,u_dot,v_dot,w_dot,phi_dot,theta_dot) 42 | 43 | # dynamics 44 | dx = u 45 | dy = v 46 | dz = w 47 | du = sin(theta) * cos(phi) * thrust/hover_thrust*g 48 | dv = -sin(phi) * thrust/hover_thrust*g 49 | dw = -g + cos(theta) * cos(phi) * thrust/hover_thrust*g 50 | dphi = (phi_cmd - phi) / tau_phi 51 | dtheta = (theta_cmd - theta) / tau_theta 52 | f_expl = vertcat(dx,dy,dz,du,dv,dw,dphi,dtheta) 53 | 54 | f_impl = sym_xdot - f_expl 55 | 56 | 57 | 58 | # constraints 59 | h_expr = sym_u 60 | 61 | # cost 62 | #W_x = np.diag([120, 120, 120, 10, 10, 10, 10, 10]) 63 | #W_u = np.diag([5000, 2000, 2000]) 64 | 65 | #expr_ext_cost_e = sym_x.transpose()* W_x * sym_x 66 | #expr_ext_cost = expr_ext_cost_e + sym_u.transpose() * W_u * sym_u 67 | 68 | 69 | # nonlinear least sqares 70 | cost_y_expr = vertcat(sym_x, sym_u) 71 | #W = block_diag(W_x, W_u) 72 | 73 | model = AcadosModel() 74 | model.f_impl_expr = f_impl 75 | model.f_expl_expr = f_expl 76 | model.x = sym_x 77 | model.xdot = sym_xdot 78 | model.u = sym_u 79 | model.cost_y_expr = cost_y_expr 80 | model.cost_y_expr_e = sym_x 81 | #model.con_h_expr = h_expr 82 | model.name = model_name 83 | #model.cost_expr_ext_cost = expr_ext_cost 84 | #model.cost_expr_ext_cost_e = expr_ext_cost_e 85 | 86 | return model -------------------------------------------------------------------------------- /scripts/generate_c_code.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosOcp, AcadosOcpSolver 2 | from quadrotor import export_quadrotor_model 3 | import numpy as np 4 | import casadi 5 | #from utils import plot_pendulum 6 | import math 7 | from scipy.linalg import block_diag 8 | 9 | def main(): 10 | # create ocp object to formulate the OCP 11 | ocp = AcadosOcp() 12 | 13 | # set model 14 | model = export_quadrotor_model() 15 | ocp.model = model 16 | 17 | Tf = 1.0 18 | 19 | nx = model.x.size()[0] 20 | nu = model.u.size()[0] 21 | 22 | N = 40 23 | 24 | # set dimensions 25 | ocp.dims.N = N 26 | 27 | # set cost 28 | W_x = np.diag([120, 120, 120, 10, 10, 10, 10, 10]) #Q_mat 29 | W_u = np.diag([5000, 2000, 2000]) #R_mat 30 | W = block_diag(W_x, W_u) 31 | ocp.cost.W_e = W_x 32 | ocp.cost.W = W 33 | 34 | # the 'EXTERNAL' cost type can be used to define general cost terms 35 | # NOTE: This leads to additional (exact) hessian contributions when using GAUSS_NEWTON hessian. 36 | #ocp.cost.cost_type = 'EXTERNAL' 37 | #ocp.cost.cost_type_e = 'EXTERNAL' 38 | ocp.cost.cost_type = 'NONLINEAR_LS' 39 | ocp.cost.cost_type_e = 'NONLINEAR_LS' 40 | ocp.model.cost_expr_ext_cost = model.x.T @ W_x @ model.x + model.u.T @ W_u @ model.u 41 | ocp.model.cost_expr_ext_cost_e = model.x.T @ W_x @ model.x 42 | 43 | 44 | # set constraints 45 | u_min = np.array([0.3, -math.pi/2, -math.pi/2]) 46 | u_max = np.array([0.9, math.pi/2, math.pi/2]) 47 | ocp.constraints.lbu = u_min 48 | ocp.constraints.ubu = u_max 49 | ocp.constraints.idxbu = np.array([0,0,0]) 50 | 51 | ocp.constraints.x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 52 | 53 | # reference trajectory (will be overwritten later) 54 | x_ref = np.zeros(nx) 55 | ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0]))) 56 | ocp.cost.yref_e = x_ref 57 | 58 | # set options 59 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 60 | # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, 61 | # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP 62 | #ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' 63 | ocp.solver_options.integrator_type = 'ERK' 64 | ocp.solver_options.qp_solver_cond_N = 5 65 | #ocp.solver_options.print_level = 0 66 | ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP 67 | 68 | 69 | # set prediction horizon 70 | ocp.solver_options.tf = Tf 71 | 72 | ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') 73 | 74 | simX = np.ndarray((N+1, nx)) 75 | simU = np.ndarray((N, nu)) 76 | 77 | status = ocp_solver.solve() 78 | ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") 79 | 80 | if status != 0: 81 | raise Exception(f'acados returned status {status}.') 82 | 83 | # get solution 84 | for i in range(N): 85 | simX[i,:] = ocp_solver.get(i, "x") 86 | simU[i,:] = ocp_solver.get(i, "u") 87 | simX[N,:] = ocp_solver.get(N, "x") 88 | 89 | #plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) 90 | 91 | if __name__ == '__main__': 92 | main() -------------------------------------------------------------------------------- /src/qr_mpc_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "acados/utils/print.h" 11 | #include "acados_c/ocp_nlp_interface.h" 12 | #include "acados_c/external_function_interface.h" 13 | #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" 14 | #include "acados/ocp_nlp/ocp_nlp_cost_ls.h" 15 | 16 | #include "blasfeo/include/blasfeo_d_aux.h" 17 | #include "blasfeo/include/blasfeo_d_aux_ext_dep.h" 18 | 19 | #include "quadrotor_model/quadrotor_model.h" 20 | #include "acados_solver_quadrotor.h" 21 | 22 | class NMPC 23 | { 24 | private: 25 | enum SystemStates{ 26 | x = 0, 27 | y = 1, 28 | z = 2, 29 | u = 3, 30 | v = 4, 31 | w = 5, 32 | phi = 6, 33 | theta = 7, 34 | }; 35 | 36 | enum ControlInputs{ 37 | thrust = 0, 38 | phi_cmd = 1, 39 | theta_cmd = 2, 40 | }; 41 | 42 | struct SolverInput{ 43 | double x0[QUADROTOR_NX]; 44 | double yref[QUADROTOR_N+1][QUADROTOR_NY]; 45 | }; 46 | 47 | struct SolverOutput{ 48 | double u0[QUADROTOR_NU]; 49 | double x1[QUADROTOR_NX]; 50 | double status, kkt_res, cpu_time; 51 | }; 52 | 53 | struct Euler{ 54 | double phi; 55 | double theta; 56 | double psi; 57 | }; 58 | 59 | // ROS subscriber and publisher 60 | ros::Subscriber local_pose_sub; 61 | ros::Subscriber local_twist_sub; 62 | ros::Publisher setpoint_pub; 63 | 64 | // ROS message variables 65 | geometry_msgs::PoseStamped local_pose; 66 | geometry_msgs::TwistStamped local_twist; 67 | Euler local_euler; 68 | Euler target_euler; 69 | mavros_msgs::AttitudeTarget attitude_target; 70 | 71 | // Acados variables 72 | SolverInput acados_in; 73 | SolverOutput acados_out; 74 | int acados_status; 75 | quadrotor_solver_capsule * mpc_capsule = quadrotor_acados_create_capsule(); 76 | 77 | // Trajectory variables 78 | std::vector> trajectory; 79 | int line_number = 0; 80 | int number_of_steps = 0; 81 | 82 | // Other variables 83 | tf::Quaternion tf_quaternion; 84 | int cout_counter = 0; 85 | double logger_time; 86 | 87 | public: 88 | 89 | NMPC(ros::NodeHandle& nh, const std::string& ref_traj) 90 | { 91 | 92 | // Pre-load the trajectory 93 | const char * c = ref_traj.c_str(); 94 | number_of_steps = readDataFromFile(c, trajectory); 95 | if (number_of_steps == 0){ 96 | ROS_WARN("Cannot load CasADi optimal trajectory!"); 97 | } 98 | else{ 99 | ROS_INFO_STREAM("Number of steps of selected trajectory: " << number_of_steps << std::endl); 100 | } 101 | 102 | // Initialize MPC 103 | int create_status = 1; 104 | create_status = quadrotor_acados_create(mpc_capsule); 105 | if (create_status != 0){ 106 | ROS_INFO_STREAM("acados_create() returned status " << create_status << ". Exiting." << std::endl); 107 | exit(1); 108 | } 109 | 110 | // ROS Subscriber & Publisher 111 | local_pose_sub = nh.subscribe("/mavros/local_position/pose", 20, &NMPC::local_pose_cb, this); 112 | local_twist_sub = nh.subscribe("/mavros/local_position/velocity_local", 20, &NMPC::local_twist_cb, this); 113 | setpoint_pub = nh.advertise("/mavros/setpoint_raw/attitude",20); 114 | 115 | // Initialize 116 | for(unsigned int i=0; i < QUADROTOR_NU; i++) acados_out.u0[i] = 0.0; 117 | for(unsigned int i=0; i < QUADROTOR_NX; i++) acados_in.x0[i] = 0.0; 118 | 119 | } 120 | 121 | void local_pose_cb(const geometry_msgs::PoseStamped::ConstPtr& pose) 122 | { 123 | local_pose.pose.position.x = pose->pose.position.x; 124 | local_pose.pose.position.y = pose->pose.position.y; 125 | local_pose.pose.position.z = pose->pose.position.z; 126 | 127 | tf::quaternionMsgToTF(pose->pose.orientation,tf_quaternion); 128 | tf::Matrix3x3(tf_quaternion).getRPY(local_euler.phi, local_euler.theta, local_euler.psi); 129 | } 130 | 131 | void local_twist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist) 132 | { 133 | local_twist.twist.linear.x = twist->twist.linear.x; 134 | local_twist.twist.linear.y = twist->twist.linear.y; 135 | local_twist.twist.linear.z = twist->twist.linear.z; 136 | } 137 | 138 | int readDataFromFile(const char* fileName, std::vector> &data) 139 | { 140 | std::ifstream file(fileName); 141 | std::string line; 142 | int number_of_lines = 0; 143 | 144 | if (file.is_open()) 145 | { 146 | while(getline(file, line)){ 147 | number_of_lines++; 148 | std::istringstream linestream( line ); 149 | std::vector linedata; 150 | double number; 151 | 152 | while( linestream >> number ){ 153 | linedata.push_back( number ); 154 | } 155 | data.push_back( linedata ); 156 | } 157 | 158 | file.close(); 159 | } 160 | else 161 | { 162 | return 0; 163 | } 164 | 165 | return number_of_lines; 166 | } 167 | 168 | void ref_cb(int line_to_read) 169 | { 170 | if (QUADROTOR_N+line_to_read+1 <= number_of_steps) // All ref points within the file 171 | { 172 | for (unsigned int i = 0; i <= QUADROTOR_N; i++) // Fill all horizon with file data 173 | { 174 | for (unsigned int j = 0; j <= QUADROTOR_NY; j++) 175 | { 176 | acados_in.yref[i][j] = trajectory[i+line_to_read][j]; 177 | } 178 | } 179 | } 180 | else if(line_to_read < number_of_steps) // Part of ref points within the file 181 | { 182 | for (unsigned int i = 0; i < number_of_steps-line_to_read; i++) // Fill part of horizon with file data 183 | { 184 | for (unsigned int j = 0; j <= QUADROTOR_NY; j++) 185 | { 186 | acados_in.yref[i][j] = trajectory[i+line_to_read][j]; 187 | } 188 | } 189 | 190 | for (unsigned int i = number_of_steps-line_to_read; i <= QUADROTOR_N; i++) // Fill the rest horizon with the last point 191 | { 192 | for (unsigned int j = 0; j <= QUADROTOR_NY; j++) 193 | { 194 | acados_in.yref[i][j] = trajectory[number_of_steps-1][j]; 195 | } 196 | } 197 | } 198 | else // none of ref points within the file 199 | { 200 | for (unsigned int i = 0; i <= QUADROTOR_N; i++) // Fill all horizon with the last point 201 | { 202 | for (unsigned int j = 0; j <= QUADROTOR_NY; j++) 203 | { 204 | acados_in.yref[i][j] = trajectory[number_of_steps-1][j]; 205 | } 206 | } 207 | } 208 | } 209 | 210 | void run() 211 | { 212 | acados_in.x0[x] = local_pose.pose.position.x; 213 | acados_in.x0[y] = local_pose.pose.position.y; 214 | acados_in.x0[z] = local_pose.pose.position.z; 215 | acados_in.x0[u] = local_twist.twist.linear.x; 216 | acados_in.x0[v] = local_twist.twist.linear.y; 217 | acados_in.x0[w] = local_twist.twist.linear.z; 218 | acados_in.x0[phi] = local_euler.phi; 219 | acados_in.x0[theta] = local_euler.theta; 220 | 221 | ocp_nlp_constraints_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in, 0, "lbx", acados_in.x0); 222 | ocp_nlp_constraints_model_set(mpc_capsule->nlp_config,mpc_capsule->nlp_dims,mpc_capsule->nlp_in, 0, "ubx", acados_in.x0); 223 | 224 | ref_cb(line_number); 225 | line_number++; 226 | 227 | for (unsigned int i = 0; i <= QUADROTOR_N; i++) 228 | { 229 | ocp_nlp_cost_model_set(mpc_capsule->nlp_config, mpc_capsule->nlp_dims, mpc_capsule->nlp_in, i, "yref", acados_in.yref[i]); 230 | } 231 | 232 | acados_status = quadrotor_acados_solve(mpc_capsule); 233 | 234 | if (acados_status != 0){ 235 | ROS_INFO_STREAM("acados returned status " << acados_status << std::endl); 236 | } 237 | 238 | acados_out.status = acados_status; 239 | acados_out.kkt_res = (double)mpc_capsule->nlp_out->inf_norm_res; 240 | 241 | ocp_nlp_get(mpc_capsule->nlp_config, mpc_capsule->nlp_solver, "time_tot", &acados_out.cpu_time); 242 | 243 | ocp_nlp_out_get(mpc_capsule->nlp_config, mpc_capsule->nlp_dims, mpc_capsule->nlp_out, 0, "u", (void *)acados_out.u0); 244 | 245 | attitude_target.thrust = acados_out.u0[0]; 246 | target_euler.phi = acados_out.u0[1]; 247 | target_euler.theta = acados_out.u0[2]; 248 | target_euler.psi = 0.00; 249 | 250 | geometry_msgs::Quaternion target_quaternion = tf::createQuaternionMsgFromRollPitchYaw(target_euler.phi, target_euler.theta, target_euler.psi); 251 | 252 | attitude_target.orientation.w = target_quaternion.w; 253 | attitude_target.orientation.x = target_quaternion.x; 254 | attitude_target.orientation.y = target_quaternion.y; 255 | attitude_target.orientation.z = target_quaternion.z; 256 | 257 | setpoint_pub.publish(attitude_target); 258 | 259 | 260 | /*Mission information cout**********************************************/ 261 | if(cout_counter > 2){ //reduce cout rate 262 | std::cout << "------------------------------------------------------------------------------" << std::endl; 263 | 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; 264 | std::cout << "x_gt: " << acados_in.x0[0] << "\ty_gt: " << acados_in.x0[1] << "\tz_gt: " << acados_in.x0[2] << std::endl; 265 | std::cout << "theta_cmd: " << target_euler.theta << "\tphi_cmd: " << target_euler.phi << "\tthrust_cmd: " << attitude_target.thrust << std::endl; 266 | std::cout << "solve_time: "<< acados_out.cpu_time << "\tkkt_res: " << acados_out.kkt_res << "\tacados_status: " << acados_out.status << std::endl; 267 | std::cout << "ros_time: " << std::fixed << ros::Time::now().toSec() << std::endl; 268 | std::cout << "------------------------------------------------------------------------------" << std::endl; 269 | cout_counter = 0; 270 | } 271 | else{ 272 | cout_counter++; 273 | } 274 | } 275 | }; 276 | 277 | int main(int argc, char **argv) 278 | { 279 | ros::init(argc, argv, "quadrotor_mpc"); 280 | ros::NodeHandle nh; 281 | std::string ref_traj; 282 | nh.getParam("/qr_mpc_node/ref_traj", ref_traj); 283 | NMPC nmpc(nh, ref_traj); 284 | ros::Rate loop_rate(40); 285 | 286 | while(ros::ok()){ 287 | nmpc.run(); 288 | ros::spinOnce(); 289 | loop_rate.sleep(); 290 | } 291 | 292 | return 0; 293 | } 294 | --------------------------------------------------------------------------------