Merge branch 'jsk-ros-pkg:master' into PR/birotor

aerial_robot/package.xml

@@ -21,6 +21,7 @@
   <exec_depend>gimbalrotor</exec_depend>
   <exec_depend>hydrus</exec_depend>
   <exec_depend>hydrus_xi</exec_depend>
+  <exec_depend>mini_quadrotor</exec_depend>
   <exec_depend>motor_test</exec_depend>
   <exec_depend>spinal</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python-is-python3</exec_depend>

aerial_robot_base/config/sensors/imu/spinal.yaml

@@ -0,0 +1,9 @@
+sensor_plugin:
+        imu:
+                imu_topic_name: imu
+                level_acc_noise_sigma: 0.05
+                z_acc_noise_sigma: 0.05
+                level_acc_bias_noise_sigma: 0.001 # (> 0 means to do the acc bias estimation by kalman filter)
+                z_acc_bias_noise_sigma: 0.001 # (= 0 means not to do the acc bias estimation by kalman filter)
+                angle_bias_noise_sigma: 0.0002 #good for EKF
+                calib_time: 2.0

robots/mini_quadrotor/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mini_quadrotor)
+
+find_package(catkin REQUIRED)
+
+catkin_package(
+  CATKIN_DEPENDS aerial_robot_control aerial_robot_estimation aerial_robot_model
+)
+
+install(DIRECTORY config launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_subdirectory(test)
+endif()

robots/mini_quadrotor/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/mini_quadrotor/config/FlightControl.yaml

@@ -0,0 +1,54 @@
+aerial_robot_control_name: aerial_robot_control/under_actuated_tilted_lqi_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
+
+  yaw:
+    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/mini_quadrotor/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/mini_quadrotor/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/mini_quadrotor/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/mini_quadrotor/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/mini_quadrotor/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/mini_quadrotor/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