[Gimbalrotor][dragonfly] create new profile for dragonfly

robots/gimbalrotor/config/dragonfly/GimbalrotorControl.yaml

@@ -0,0 +1,49 @@
+aerial_robot_control_name: aerial_robot_control/gimbalrotor_controller
+
+controller:
+  torque_allocation_matrix_inv_pub_interval: 0.05
+  wrench_allocation_matrix_pub_interval: 0.1
+  gimbal_calc_in_fc : true
+
+  xy:
+    p_gain: 6.0
+    i_gain: 0.05
+    d_gain: 1.5
+    limit_sum: 4.0
+    limit_p: 12.0
+    limit_i: 12.0
+    limit_d: 12.0
+
+  z:
+    p_gain: 5.0
+    i_gain: 1.0
+    d_gain: 2.5
+    limit_err_p: 1.0
+    limit_sum: 25.0 # N for clamping thrust force
+    limit_p: 25.0
+    limit_i: 25.0
+    limit_d: 25.0
+    force_landing_descending_rate: -0.5
+
+  roll_pitch:
+    p_gain: 30.0
+    i_gain: 5.0
+    d_gain: 14.0
+    limit_sum: 20.0
+    limit_p: 20.0
+    limit_i: 20.0
+    limit_d: 20.0
+
+    start_rp_integration_height: 0.01
+
+  yaw:
+    p_gain: 8.0
+    i_gain: 1.0
+    d_gain: 4.0
+    limit_err_p: 0.4
+    limit_sum: 20.0
+    limit_p: 20.0
+    limit_i: 20.0
+    limit_d: 20.0
+
+    need_d_control: true

robots/gimbalrotor/config/dragonfly/GimbalrotorControl_sim.yaml

@@ -0,0 +1,50 @@
+aerial_robot_control_name: aerial_robot_control/gimbalrotor_controller
+
+controller:
+  torque_allocation_matrix_inv_pub_interval: 0.05
+  wrench_allocation_matrix_pub_interval: 0.1
+  gimbal_calc_in_fc : true
+  gimbal_dof : 2
+
+  xy:
+    p_gain: 3.0
+    i_gain: 0.05
+    d_gain: 1.5
+    limit_sum: 4.0
+    limit_p: 12.0
+    limit_i: 12.0
+    limit_d: 12.0
+
+  z:
+    p_gain: 5.0
+    i_gain: 1.0
+    d_gain: 2.5
+    limit_err_p: 1.0
+    limit_sum: 25.0 # N for clamping thrust force
+    limit_p: 25.0
+    limit_i: 25.0
+    limit_d: 25.0
+    force_landing_descending_rate: -0.5
+
+  roll_pitch:
+    p_gain: 22.0
+    i_gain: 1.0
+    d_gain: 14.0
+    limit_sum: 20.0
+    limit_p: 20.0
+    limit_i: 20.0
+    limit_d: 20.0
+
+    start_rp_integration_height: 0.01
+
+  yaw:
+    p_gain: 5.0
+    i_gain: 1.0
+    d_gain: 4.0
+    limit_err_p: 0.4
+    limit_sum: 20.0
+    limit_p: 20.0
+    limit_i: 20.0
+    limit_d: 20.0
+
+    need_d_control: true

robots/gimbalrotor/config/dragonfly/Servo.yaml

@@ -0,0 +1,70 @@
+servo_controller:
+
+  joints:
+    state_sub_topic: servo/states
+    ctrl_pub_topic: servo/target_states
+    torque_pub_topic: servo/torque_enable
+
+    angle_scale: 0.00076699
+    zero_point_offset: 2047
+    torque_scale: 1.0 # TODO please update the scale
+
+    filter_flag: true
+    sample_freq: 50.0 # 50Hz
+    cutoff_freq: 10.0 # 10Hz, test
+
+    controller1:
+      id: 7
+      name: head_joint
+      angle_sgn: -1
+      simulation:
+        pid: {p: 50.0, i: 0.01, d: 1.0}
+        init_value: 0
+        type: effort_controllers/JointPositionController
+
+    controller2:
+      id: 8
+      name: tail_joint
+      angle_sgn: -1
+      simulation:
+        pid: {p: 50.0, i: 0.01, d: 1.0}
+        init_value: 0
+        type: effort_controllers/JointPositionController
+
+  gimbals:
+    state_sub_topic: servo/states
+    ctrl_pub_topic: servo/target_states
+    torque_pub_topic: servo/torque_enable
+
+    angle_sgn: -1
+    angle_scale: 0.001534
+    zero_point_offset: 2047
+
+    simulation:
+      pid: {p: 5.0, i: 0.1, d: 0.1, i_clamp_max: 2.0, i_clamp_min: -2.0}
+      init_value: 0
+      type: effort_controllers/JointPositionController
+
+    controller1:
+      id: 1
+      name: gimbal1_roll
+
+    controller2:
+      id: 2
+      name: gimbal1_pitch
+
+    controller3:
+      id: 3
+      name: gimbal2_roll
+
+    controller4:
+      id: 4
+      name: gimbal2_pitch
+
+    controller5:
+      id: 5
+      name: gimbal3_roll
+
+    controller6:
+      id: 6
+      name: gimbal3_pitch

robots/gimbalrotor/robots/dragonfly/gimbalrotor.gazebo.xacro

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="gimbalrotor" >
+  <xacro:arg name="robot_name" default="gimbalrotor" />
+
+  <xacro:include filename="$(find gimbalrotor)/robots/dragonfly/gimbalrotor.urdf.xacro" />
+
+  <!-- gazebo plugin for default controller and sensors -->
+  <xacro:include filename="$(find aerial_robot_simulation)/xacro/spinal.gazebo.xacro" />
+  <xacro:gazebo_spinal robot_name="$(arg robot_name)" mag_frame="fc" />
+
+</robot>

robots/gimbalrotor/robots/dragonfly/gimbalrotor.urdf.xacro

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="gimbalrotor" >
+
+  <xacro:include filename="$(find gimbalrotor)/urdf/dragonfly/gimbalrotor.urdf.xacro" />
+  <xacro:include filename="$(find gimbalrotor)/urdf/dragonfly/common.xacro" />
+
+  <xacro:gimbalrotor_center_link child1="1" child2="2" child3="3"/>
+  <xacro:gimbalrotor_gimbal_link self="1" rotor_direction="1"/>
+  <xacro:gimbalrotor_gimbal_link self="2" rotor_direction="-1"/>
+  <xacro:gimbalrotor_gimbal_link self="3" rotor_direction="1"/>
+  <xacro:gimbalrotor_head_link/>
+  <xacro:gimbalrotor_tail_link/>
+
+</robot>

robots/gimbalrotor/urdf/dragonfly/common.xacro

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="gimbalrotor_common" >
+
+  <!-- general attribute -->
+  <baselink name="fc" />
+  <thrust_link name="thrust" />
+
+  <m_f_rate value="-0.0172" /> <!-- drug torque rate -->
+  <xacro:property name="max_force" value="18.0" /> <!-- [N] -->
+  <xacro:property name="min_force" value="1.0" /> <!-- [N] -->
+
+
+  <!-- friction -->
+  <xacro:macro name="friction" params="self">
+    <gazebo reference="link${self}">
+      <mu1>0.1</mu1>
+      <mu2>0.1</mu2>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="damping_factor" params="link">
+    <gazebo reference="${link}">
+      <dampingFactor>0.00</dampingFactor>
+    </gazebo>
+  </xacro:macro>
+</robot>