[Beetle][NMPC] The latest version of real machine

.gitignore

@@ -111,3 +111,8 @@ tags
 # mujoco
 MUJOCO_LOG.txt
 robots/**/mujoco
+
+# acados
+*c_generated_code/
+*acados_ocp.json
+*model.json

.pre-commit-config.yaml

@@ -9,6 +9,7 @@ repos:
       - id: end-of-file-fixer
       - id: trailing-whitespace
       - id: check-added-large-files
+        args: ['--maxkb=10000']
 
     # Python
   - repo: https://github.com/psf/black

3rdpary/acados/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(acados)
+include(ExternalProject)
+
+#message(WARNING "${CMAKE_INSTALL_PREFIX}")
+
+ExternalProject_Add(
+  acados
+  GIT_REPOSITORY https://github.com/acados/acados.git
+  GIT_TAG fe308a5
+  CMAKE_ARGS -DACADOS_WITH_QPOASES=ON  -DACADOS_INSTALL_DIR=${CMAKE_INSTALL_PREFIX}
+  INSTALL_COMMAND echo "install"
+  LOG_DOWNLOAD ON
+  LOG_CONFIGURE ON
+  LOG_BUILD ON
+  LOG_INSTALL_ON
+  )
+
+install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/acados-prefix/src/acados-build)")

3rdpary/acados/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package>
+  <name>acados</name>
+  <version>0.0.0</version>
+  <description>acados ROS Wrapper</description>
+  <maintainer email="[email protected]">Moju Zhao</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <build_depend>git</build_depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+
+</package>
+

aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h

@@ -19,6 +19,9 @@ class AerialRobotBase
   void mainFunc(const ros::TimerEvent & e);
 
  private:
+  bool param_verbose_;
+  double main_rate_;
+
   ros::NodeHandle nh_;
   ros::NodeHandle nhp_;
   ros::Timer main_timer_;

aerial_robot_base/src/aerial_robot_base.cpp

@@ -6,11 +6,8 @@ AerialRobotBase::AerialRobotBase(ros::NodeHandle nh, ros::NodeHandle nh_private)
     navigator_loader_("aerial_robot_control", "aerial_robot_navigation::BaseNavigator")
 {
 
-  bool param_verbose;
-  nhp_.param ("param_verbose", param_verbose, true);
-
-  double main_rate;
-  nhp_.param ("main_rate", main_rate, 0.0);
+  nhp_.param ("param_verbose", param_verbose_, true);
+  nhp_.param ("main_rate", main_rate_, 0.0);
 
   // robot model
   robot_model_ros_ = boost::make_shared<aerial_robot_model::RobotModelRos>(nh_, nhp_);
@@ -38,29 +35,29 @@ AerialRobotBase::AerialRobotBase(ros::NodeHandle nh, ros::NodeHandle nh_private)
       ROS_DEBUG("use default class for flight navigation: aerial_robot_navigation::BaseNavigator");
       navigator_ = boost::make_shared<aerial_robot_navigation::BaseNavigator>();
     }
-  navigator_->initialize(nh_, nhp_, robot_model, estimator_, 1 / main_rate);
+  navigator_->initialize(nh_, nhp_, robot_model, estimator_, 1 / main_rate_);
 
   //  controller
   try
     {
       std::string aerial_robot_control_name;
       nh_.param ("aerial_robot_control_name", aerial_robot_control_name, std::string("aerial_robot_control/flatness_pid"));
       controller_ = controller_loader_.createInstance(aerial_robot_control_name);
-      controller_->initialize(nh_, nhp_, robot_model, estimator_, navigator_, 1 / main_rate);
+      controller_->initialize(nh_, nhp_, robot_model, estimator_, navigator_, 1 / main_rate_);
     }
   catch(pluginlib::PluginlibException& ex)
     {
       ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
     }
 
-  if(param_verbose) cout << nhp_.getNamespace() << ": main_rate is " << main_rate << endl;
-  if(main_rate <= 0)
+  if(param_verbose_) cout << nhp_.getNamespace() << ": main_rate is " << main_rate_ << endl;
+  if(main_rate_ <= 0)
     ROS_ERROR_STREAM("mian rate is negative, can not run the main timer");
   else
     {
       // note1: separate the thread for main control (including navigation) loop to guarantee a relatively stable loop rate
 
-      ros::TimerOptions ops(ros::Duration(1.0 / main_rate),
+      ros::TimerOptions ops(ros::Duration(1.0 / main_rate_),
                             boost::bind(&AerialRobotBase::mainFunc, this, _1),
                             &main_loop_queue_);
       main_timer_ = nhp_.createTimer(ops);
@@ -87,6 +84,17 @@ AerialRobotBase::~AerialRobotBase()
 
 void AerialRobotBase::mainFunc(const ros::TimerEvent & e)
 {
+  if (!e.last_real.isZero())
+  {
+      double dt_real = e.current_real.toSec() - e.last_real.toSec();
+      double tolerance = 0.05;  // 5% tolerance. This value is set based on experience.
+      double dt_desire = (1.0 + tolerance) * 1.0 / main_rate_;
+      if (dt_real > dt_desire)
+      {
+          ROS_WARN("main loop rate is too low: (ts_real) %f s > (ts_desire with %2f%% tolerance) %f s ", dt_real,
+                   tolerance * 100, dt_desire);
+      }
+  }
   navigator_->update();
   controller_->update();
 }

aerial_robot_control/CMakeLists.txt

@@ -16,9 +16,15 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Eigen3 REQUIRED)
 
+
+#list(APPEND CMAKE_PREFIX_PATH "~/acados/install/cmake")
+find_package(acados) # if we use REQUIED, then will cause OpenBLAS and FortranLib Find_Package error
+
+
 generate_dynamic_reconfigure_options(
   cfg/PID.cfg
   cfg/LQI.cfg
+  cfg/NMPC.cfg
   )
 
 ### Temporary soulition because of https://github.com/ros/ros_comm/issues/1404
@@ -51,7 +57,7 @@ include_directories(
 ### control utils
 add_library(control_utils
   src/control/utils/care.cpp
-  )
+)
 target_link_libraries(control_utils ${EIGEN3_LIBRARIES})
 
 ### flight control plugin
@@ -60,9 +66,25 @@ add_library(flight_control_pluginlib
   src/control/fully_actuated_controller.cpp
   src/control/under_actuated_controller.cpp
   src/control/under_actuated_lqi_controller.cpp
-  src/control/under_actuated_tilted_lqi_controller.cpp)
-
-target_link_libraries(flight_control_pluginlib ${catkin_LIBRARIES} control_utils)
+  src/control/under_actuated_tilted_lqi_controller.cpp
+  src/nmpc/under_act_body_rate/mpc_solver.cpp
+  src/nmpc/under_act_body_rate/nmpc_controller.cpp
+  src/nmpc/under_act_full/mpc_solver.cpp
+  src/nmpc/under_act_full/nmpc_controller.cpp
+  src/nmpc/over_act_no_servo_delay/nmpc_solver.cpp
+  src/nmpc/over_act_no_servo_delay/nmpc_controller.cpp
+  src/nmpc/over_act_full/nmpc_solver.cpp
+  src/nmpc/over_act_full/nmpc_controller.cpp
+  src/nmpc/over_act_full_i_term/nmpc_solver.cpp
+  src/nmpc/over_act_full_i_term/nmpc_controller.cpp
+  )
+target_link_libraries(flight_control_pluginlib ${catkin_LIBRARIES} control_utils acados
+        ${PROJECT_SOURCE_DIR}/include/aerial_robot_control/nmpc/under_act_body_rate/c_generated_code/libacados_ocp_solver_qd_body_rate_model.so
+        ${PROJECT_SOURCE_DIR}/include/aerial_robot_control/nmpc/under_act_full/c_generated_code/libacados_ocp_solver_qd_full_model.so
+        ${PROJECT_SOURCE_DIR}/include/aerial_robot_control/nmpc/over_act_no_servo_delay/c_generated_code/libacados_ocp_solver_beetle_no_servo_delay_model.so
+        ${PROJECT_SOURCE_DIR}/include/aerial_robot_control/nmpc/over_act_full/c_generated_code/libacados_ocp_solver_beetle_full_model.so
+        ${PROJECT_SOURCE_DIR}/include/aerial_robot_control/nmpc/over_act_full_i_term/c_generated_code/libacados_ocp_solver_beetle_full_w_disturb_model.so
+        )
 add_dependencies(flight_control_pluginlib  ${PROJECT_NAME}_gencfg)
 
 ### flight navigation

aerial_robot_control/cfg/NMPC.cfg

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+PACKAGE = "aerial_robot_control"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+from aerial_robot_msgs.msg import DynamicReconfigureLevels
+
+gen = ParameterGenerator()
+
+gen.add("nmpc_flag", bool_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_FLAG, "NMPC Control Weight Flag", False)
+gen.add("Qp_xy", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_P_XY, "NMPC Qp_xy", 300, 0, 2000)
+gen.add("Qp_z", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_P_Z, "NMPC Qp_z", 400, 0, 2000)
+gen.add("Qv_xy", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_V_XY, "NMPC Qv_xy", 10, 0, 200)
+gen.add("Qv_z", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_V_Z, "NMPC Qv_z", 10, 0, 200)
+gen.add("Qq_xy", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_Q_XY, "NMPC Qq_xy", 300, 0, 2000)
+gen.add("Qq_z", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_Q_Z, "NMPC Qq_z", 500, 0, 2000)
+gen.add("Qw_xy", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_W_XY, "NMPC Qw_xy", 5, 0, 100)
+gen.add("Qw_z", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_W_Z, "NMPC Qw_z", 5, 0, 100)
+gen.add("Qa", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_Q_A, "NMPC Qa", 1, 0, 10)
+gen.add("Rt", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_R_T, "NMPC Rt", 1, 0, 10)
+gen.add("Rac_d", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_R_AC_D, "NMPC Rac_d", 250, 0, 2000)
+
+gen.add("i_gain_flag", bool_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_FLAG, "NMPC I Gain Flag", False)
+gen.add("i_gain_x", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_X, "NMPC I Gain x", 3, 0, 20)
+gen.add("i_gain_y", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_Y, "NMPC I Gain y", 3, 0, 20)
+gen.add("i_gain_z", int_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_Z, "NMPC I Gain z", 3, 0, 20)
+gen.add("i_gain_roll", double_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_ROLL, "NMPC I Gain roll", 0.5, 0, 3)
+gen.add("i_gain_pitch", double_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_PITCH, "NMPC I Gain pitch", 0.5, 0, 3)
+gen.add("i_gain_yaw", double_t, DynamicReconfigureLevels.RECONFIGURE_NMPC_I_GAIN_YAW, "NMPC I Gain yaw", 0.5, 0, 3)
+
+exit(gen.generate(PACKAGE, "aerial_robot_control", "NMPC"))

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -572,4 +572,4 @@ namespace aerial_robot_navigation
         ROS_INFO_STREAM("[" << nh.getNamespace() << "] " << param_name << ": " << param);
     }
   };
-};
+};  // namespace aerial_robot_navigation

aerial_robot_control/include/aerial_robot_control/nmpc/base_mpc_controller.h

@@ -0,0 +1,8 @@
+//
+// Created by li-jinjie on 23-11-25.
+//
+
+#ifndef AERIAL_ROBOT_CONTROL_BASE_MPC_CONTROLLER_H
+#define AERIAL_ROBOT_CONTROL_BASE_MPC_CONTROLLER_H
+
+#endif  // AERIAL_ROBOT_CONTROL_BASE_MPC_CONTROLLER_H

aerial_robot_control/include/aerial_robot_control/nmpc/base_mpc_solver.h

@@ -0,0 +1,8 @@
+//
+// Created by li-jinjie on 23-11-25.
+//
+
+#ifndef AERIAL_ROBOT_CONTROL_BASE_MPC_SOLVER_H
+#define AERIAL_ROBOT_CONTROL_BASE_MPC_SOLVER_H
+
+#endif  // AERIAL_ROBOT_CONTROL_BASE_MPC_SOLVER_H

aerial_robot_control/include/aerial_robot_control/nmpc/i_term.h

@@ -0,0 +1,67 @@
+//
+// Created by li-jinjie on 24-3-1.
+//
+
+#ifndef AERIAL_ROBOT_CONTROL_I_TERM_H
+#define AERIAL_ROBOT_CONTROL_I_TERM_H
+
+#endif  // AERIAL_ROBOT_CONTROL_I_TERM_H
+
+#pragma once
+
+#include <algorithm>
+
+namespace aerial_robot_control
+{
+class ITerm
+{
+public:
+  ITerm() = default;
+  ~ITerm() = default;
+
+  void initialize(double i_gain, double u_limit, double dt)
+  {
+    i_gain_ = i_gain;
+    u_limit_ = u_limit;
+    dt_ = dt;
+    i_term_ = 0;
+    error_last_round_ = 0;
+  }
+
+  void setIGain(double i_gain)
+  {
+    i_gain_ = i_gain;
+  }
+
+  double update(double error)  // y - y_ref
+  {
+    // update the integrator using trapazoidal rule
+    i_term_ += (dt_ / 2) * (error + error_last_round_);
+
+    double u = i_gain_ * i_term_;
+
+    double u_sat = std::max(std::min(u, u_limit_), -u_limit_);
+
+    // integral anti-windup: adjust integrator to keep u out of saturation
+    if (i_gain_ != 0)
+      i_term_ += (dt_ / i_gain_) * (u_sat - u);
+
+    error_last_round_ = error;
+
+    return u_sat;
+  }
+
+  void reset()
+  {
+    i_term_ = 0;
+    error_last_round_ = 0;
+  }
+
+private:
+  double i_gain_;
+  double u_limit_;
+  double dt_;
+  double i_term_;
+  double error_last_round_;
+};
+}  // namespace aerial_robot_control