[WIP] implement mpc control for simple quadrotor

robots/delta/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(delta)
+
+set(CMAKE_CXX_STANDARD 17)
+
+add_compile_options(-std=c++17)
+
+find_package(catkin REQUIRED COMPONENTS
+  aerial_robot_control
+  aerial_robot_estimation
+  aerial_robot_model
+  aerial_robot_msgs
+  casadi_eigen
+  pluginlib
+  roscpp
+  )
+
+find_package(Eigen3 REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(casadi REQUIRED)
+
+catkin_package(
+  CATKIN_DEPENDS aerial_robot_control aerial_robot_estimation aerial_robot_model aerial_robot_msgs casadi_eigen pluginlib roscpp
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  # ${EIGEN3_INCLUDE_DIRS}
+  )
+
+add_library(delta_controller src/control/delta_controller.cpp)
+target_link_libraries(delta_controller ${catkin_LIBRARIES} casadi)
+
+add_executable(delta_model src/model/delta_model.cpp)
+target_link_libraries(delta_model ${catkin_LIBRARIES} ${pinocchio_LIBRARIES})

robots/delta/config/Battery.yaml

@@ -0,0 +1,5 @@
+bat_info:
+        bat_cell: 3
+        bat_resistance: 0.04 # [m Ohm]
+        hovering_current: 15 #[A]
+        low_voltage_thre: 10 #[100%]

robots/delta/config/FlightControl.yaml

@@ -0,0 +1,66 @@
+aerial_robot_control_name: aerial_robot_control/delta_controller
+
+controller:
+  xy:
+    p_gain: 5.0
+    i_gain: 0.05
+    d_gain: 3.0
+    limit_sum: 3
+    limit_p: 3
+    limit_i: 1.5
+    limit_d: 3
+
+  z:
+    p_gain: 5.0
+    i_gain: 2.5
+    d_gain: 5.0
+    limit_err_p: 1.0
+    limit_sum: 10 # N for clamping thrust force
+    limit_p: 20 # m / s^2
+    limit_i: 40 # m / s^2
+    limit_d: 20 # m / s^2
+    landing_err_z: -0.2
+    force_landing_descending_rate: -0.6
+
+  roll_pitch:
+    p_gain: 15
+    i_gain: 0.3
+    d_gain: 5 # 5: good, 7: deverge in the takeoff phase
+    limit_sum: 10
+    limit_p: 10
+    limit_i: 10
+    limit_d: 10
+
+  yaw:
+    p_gain: 10.0
+    i_gain: 0.1
+    d_gain: 5.0
+    limit_sum: 6.0 # N for clamping thrust force
+    limit_p: 4.0
+    limit_i: 4.0
+    limit_d: 4.0
+    limit_err_p: 0.4
+    need_d_control: false
+
+  # LQI gain generator
+  lqi:
+    gain_generate_rate: 15.0
+    gyro_moment_compensation: true
+    clamp_gain: true
+
+    roll_pitch_p: 20
+    roll_pitch_i: 0.5
+    roll_pitch_d: 0.15
+
+    yaw_p: 60
+    yaw_i: 0.5
+    yaw_d: 1.0
+
+    r1: 1.0
+    r2: 1.0
+    r3: 1.0
+    r4: 1.0
+
+    trans_constraint_weight: 100
+    att_control_weight: 1
+

robots/delta/config/MPC.yaml

@@ -0,0 +1,21 @@
+controller:
+  yaw_p_gain: 1.0
+  holizon: 2.0
+  n_grid: 10
+  nx: 12
+  nu: 4
+
+  q_rx: 40.0
+  q_ry: 40.0
+  q_rz: 10.0
+  q_vx: 0.0
+  q_vy: 0.0
+  q_vz: 0.0
+  q_alpha_x: 0.0
+  q_alpha_y: 0.0
+  q_alpha_z: 10.0
+  q_omega_x: 0.0
+  q_omega_y: 0.0
+  q_omega_z: 1.0
+
+  u_thrust: 0.0

robots/delta/config/MotorInfo.yaml

@@ -0,0 +1,24 @@
+motor_info: # TODO: update
+        min_pwm: 0.6 # 1200 [ms]
+        max_pwm: 0.975 # 1950 [ms]
+        min_thrust: 0 #  [N]
+        force_landing_thrust: 1.0 #[N]
+        pwm_conversion_mode: 1 # Polynominal Mode
+        vel_ref_num: 2
+        ref1:
+                voltage: 11
+                max_thrust: 7.2 # N
+                polynominal4: -168.952346    #  -0.0168952346 x10^4
+                polynominal3: 308.7473666    #   0.3087473666 x10^3
+                polynominal2: -203.28702334  #  -2.0328702334 x10^2
+                polynominal1: 106.787265636  #  10.6787265636 x10
+                polynominal0: 56.3518258750  #  56.3518258750 x1
+        ref2:
+                voltage: 12
+                max_thrust: 8.0 # N
+                polynominal4: -39.838825     #  -0.0039838825 x10^4
+                polynominal3: 112.1897517    #   0.1121897517 x10^3
+                polynominal2: -101.84075291  #  -1.0184075291 x10^2
+                polynominal1:  81.582961230  #   8.1582961230 x10
+                polynominal0:  56.9010618089 #  56.9010618089 x1
+

robots/delta/config/NavigationConfig.yaml

@@ -0,0 +1,30 @@
+# Teleop Basic Param
+
+navigation:
+  xy_control_mode: 0
+  # World Pos Control Mode: 0
+  # World Vel Control Mode: 2
+  # Local Vel Control Mode: 3
+  # Attitude Control Mode: 4
+
+
+  takeoff_height: 1.0 #1.0
+  outdoor_takeoff_height: 1.5
+
+  # teleop operation
+  max_target_vel: 0.5
+  joy_target_vel_interval: 0.005  # 0.2 / 20 = 0.01, 0.005 ~ 0.01  m/s
+  joy_target_z_interval: 0.02
+  max_target_yaw_rate: 0.1 #  0.05
+  teleop_local_frame: fc
+
+  cmd_vel_lev2_gain : 2.0
+  nav_vel_limit : 1.0
+
+  gain_tunning_mode: 0
+  max_target_tilt_angle: 0.2
+  cmd_angle_lev2_gain : 1.5
+
+  # gps waypoint
+  gps_waypoint_threshold: 3.0
+  gps_waypoint_check_du: 1.0

robots/delta/config/RobotModel.yaml

@@ -0,0 +1,11 @@
+# robot model plugin
+robot_model_plugin_name: multirotor_robot_model
+
+wrench_mat_det_thre: 0
+
+# feasible control configuration
+fc_t_min_thre: 0.01
+fc_rp_min_thre: 1.8
+rp_position_margin_thre: 0.13 # deprecated
+
+

robots/delta/config/RvizInit.yaml

@@ -0,0 +1,9 @@
+zeros:
+  rotor1: 0
+  rotor2: 0
+  rotor3: 0
+  rotor4: 0
+  rotor5: 0
+  rotor6: 0
+  rotor7: 0
+  rotor8: 0

robots/delta/config/Simulation.yaml

@@ -0,0 +1,28 @@
+# overwrite parameter for simulation
+
+sensor_plugin:
+  mocap:
+    ground_truth_sub_name: ground_truth
+  gps:
+    estimate_mode: 4 #1 << 2
+    ned_flag: true # hector_gazebo_plugins/GPS
+
+# ground truth parameter
+# global namespace
+simulation:
+  # gaussian noise
+  ground_truth_pos_noise: 0.001
+  ground_truth_vel_noise: 0.005
+  ground_truth_rot_noise: 0.001
+  ground_truth_angular_noise: 0.005
+
+  # drift model: e^(-dt * drift_frequency) * curr_drift + dt * gaussian_kernel(sqrt( 2 * drift_frequency) * drfit))
+  # drift diviation:
+  ground_truth_vel_drift: 0
+  ground_truth_rot_drift: 0
+  ground_truth_angular_drift: 0
+
+  # drift frequency:
+  ground_truth_vel_drift_frequency: 0
+  ground_truth_rot_drift_frequency: 0
+  ground_truth_angular_drift_frequency: 0

robots/delta/config/StateEstimation.yaml

@@ -0,0 +1,47 @@
+# Sensor Fuser Loader
+estimation:
+        # sensor plugin
+        sensor_list:
+        - sensor_plugin/imu
+        - sensor_plugin/mocap
+
+        # egomotion estimation
+        egomotion_list:
+        - kalman_filter/kf_pos_vel_acc
+        - kalman_filter/kf_pos_vel_acc
+        - kalman_filter/kf_pos_vel_acc
+
+        # experiment estimation
+        experiment_list:
+        - kalman_filter/kf_pos_vel_acc
+        - kalman_filter/kf_pos_vel_acc
+        - kalman_filter/kf_pos_vel_acc
+
+        # specific id(int) of sensor fuser, axis for kf
+        fuser_egomotion_id1: 8 # 1 << 3
+        fuser_egomotion_id2: 16 # 1 << 4
+        fuser_egomotion_id3: 32 # 1 << 5
+
+        fuser_experiment_id1: 8 # 1 << 3
+        fuser_experiment_id2: 16 # 1 << 4
+        fuser_experiment_id3: 32 # 1 << 5
+
+        # specific name of sensor fuser
+        fuser_egomotion_name1: /ee/x
+        fuser_egomotion_name2: /ee/y
+        fuser_egomotion_name3: /ee/z
+
+        fuser_experiment_name1: /ex/x
+        fuser_experiment_name2: /ex/y
+        fuser_experiment_name3: /ex/z
+
+
+# EGOMOTION_ESTIMATION_MODE = 0
+# EXPERIMENT_MODE = 1
+# GROUND_TRUTH_MODE = 2
+
+sensor_plugin:
+        imu:
+                estimate_mode: 3 # 1<<0 + 1<<1
+        mocap:
+                estimate_mode: 6 # 1<<1 + 1<<2

robots/delta/config/mujoco_model.yaml

@@ -0,0 +1,7 @@
+package:
+  ["mini_quadrotor"]
+
+mini_quadrotor:
+  input: ["urdf/robot.urdf.xacro"]
+  filename: [""]
+  meshdir: "urdf/mesh"