Hack/force sensor

rotors_description/urdf/component_snippets.xacro

@@ -261,18 +261,6 @@
     </gazebo>
   </xacro:macro>
 
-  <!-- =============================================================== -->
-  <!-- ==================== ROS INTERFACE MACRO ====================== -->
-  <!-- =============================================================== -->
-  <!-- <xacro:macro 
-  		name="ros_interface_plugin_macro"
-  		params="namespace">
-    <gazebo>
-      <plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so">
-        <robotNamespace>${namespace}</robotNamespace>
-      </plugin>
-    </gazebo>
-  </xacro:macro>-->
 
   <!-- =============================================================== -->
   <!-- ================== MAVLINK INTERFACE MACRO ==================== -->
@@ -311,8 +299,8 @@
       mass_imu_sensor gyroscope_noise_density gyroscope_random_walk
       gyroscope_bias_correlation_time gyroscope_turn_on_bias_sigma
       accelerometer_noise_density accelerometer_random_walk
-      accelerometer_bias_correlation_time accelerometer_turn_on_bias_sigma
-      *inertia *origin">
+      accelerometer_bias_correlation_time accelerometer_turn_on_bias_sigma 
+      measurement_divisor *inertia *origin">
     <!-- IMU link -->
     <link name="${namespace}/imu${imu_suffix}_link">
       <inertial>
@@ -343,6 +331,7 @@
         <accelerometerRandomWalk>${accelerometer_random_walk}</accelerometerRandomWalk> <!-- Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)] -->
         <accelerometerBiasCorrelationTime>${accelerometer_bias_correlation_time}</accelerometerBiasCorrelationTime> <!-- Accelerometer bias correlation time constant [s] -->
         <accelerometerTurnOnBiasSigma>${accelerometer_turn_on_bias_sigma}</accelerometerTurnOnBiasSigma> <!-- Accelerometer turn on bias standard deviation [m/s^2] -->
+        <measurementDivisor>${measurement_divisor}</measurementDivisor> <!-- only every (seq % measurementDivisor) == 0 measurement is published [int] -->
       </plugin>
     </gazebo>
   </xacro:macro>
@@ -787,7 +776,8 @@
       accelerometer_noise_density="0.004"
       accelerometer_random_walk="0.006"
       accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.1960">
+      accelerometer_turn_on_bias_sigma="0.1960" 
+      measurement_divisor="1">
       <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
       <origin xyz="0.015 0 0.0113" rpy="0 0 0" />
     </xacro:imu_plugin_macro>
@@ -808,7 +798,8 @@
       accelerometer_noise_density="0.0"
       accelerometer_random_walk="0.0"
       accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.0">
+      accelerometer_turn_on_bias_sigma="0.0"
+      measurement_divisor="1">
       <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:imu_plugin_macro>
@@ -860,7 +851,8 @@
       accelerometer_noise_density="0.004"
       accelerometer_random_walk="0.006"
       accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.1960">
+      accelerometer_turn_on_bias_sigma="0.1960"
+      measurement_divisor="1">
       <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:imu_plugin_macro>

rotors_description/urdf/firefly.xacro

@@ -25,7 +25,7 @@
   <xacro:property name="rotor_velocity_slowdown_sim" value="10" />
   <xacro:property name="use_mesh_file" value="true" />
   <xacro:property name="mesh_file" value="package://rotors_description/meshes/firefly.dae" />
-  <xacro:property name="mass" value="1.5" /> <!-- [kg] -->
+  <xacro:property name="mass" value="3.0" /> <!-- [kg] -->
   <xacro:property name="body_width" value="0.1" /> <!-- [m] -->
   <xacro:property name="body_height" value="0.16" /> <!-- [m] -->
   <xacro:property name="mass_rotor" value="0.005" /> <!-- [kg] -->

rotors_description/urdf/omav.xacro

@@ -56,6 +56,10 @@
     <xacro:property name="rotor_drag_coefficient" value="${8.06428e-05}"/>
     <xacro:property name="rolling_moment_coefficient" value="0.000001"/>
 
+    <xacro:property name="tilt_p_gain" value="1.0"/>
+    <xacro:property name="tilt_i_gain" value="0.0"/>
+    <xacro:property name="tilt_d_gain" value="0.1"/>
+
     <!-- Property Blocks -->
     <xacro:property name="body_inertia">
         <inertia ixx="0.0075" ixy="${-3.4208e-05}" ixz="${2.4695e-05}" iyy="0.010939" iyz="${-3.8826e-06}" izz="0.01369"/>
@@ -111,7 +115,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="Black">
+    color="Black"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="0 -${arm_length} 0" rpy="0 0 4.71238898038"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>
@@ -137,7 +144,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="Black">
+    color="Black"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="0 ${arm_length} 0" rpy="0 0 1.57079632679"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>
@@ -163,7 +173,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="Orange">
+    color="Orange"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="${cos30*arm_length} ${sin30*arm_length} 0" rpy="0 0 0.52359877559"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>
@@ -188,7 +201,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="DarkGrey">
+    color="DarkGrey"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="-${cos30*arm_length} -${sin30*arm_length} 0" rpy="0 0 3.66519142919"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>
@@ -213,7 +229,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="Orange">
+    color="Orange"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="${cos30*arm_length} -${sin30*arm_length} 0" rpy="0 0 5.75958653158"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>
@@ -238,7 +257,10 @@
     rolling_moment_coefficient="${rolling_moment_coefficient}"
     mesh="propeller"
     mesh_tilt_unit="${mesh_file_tilt_unit}"
-    color="DarkGrey">
+    color="DarkGrey"
+    p_gain="${tilt_p_gain}"
+    i_gain="${tilt_i_gain}"
+    d_gain="${tilt_d_gain}">
     <origin xyz="-${cos30*arm_length} ${sin30*arm_length} 0" rpy="0 0 2.61799387799"/>
     <xacro:insert_block name="rotor_inertia" />
   </xacro:double_tilt_unit>

rotors_description/urdf/overactuated_base.xacro

@@ -72,7 +72,7 @@
 
   <!-- Tilting rotor unit link and joint. -->
   <xacro:macro name="single_tilt_unit"
-    params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit *origin *inertia">
+    params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number tilt_unit_motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit p_gain i_gain d_gain *origin *inertia">
 
     <!-- Rotor tilt_unit -->
     <joint name="${robot_namespace}/tilt_unit_${motor_number}_revolutejoint" type="continuous">
@@ -108,13 +108,13 @@
         <turningDirection>ccw</turningDirection>
         <motorType>position</motorType>
         <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
-        <motorNumber>${motor_number+12}</motorNumber>
+        <motorNumber>${tilt_unit_motor_number}</motorNumber>
         <motorSpeedPubTopic>/tilt_motor_${motor_number}/velocity</motorSpeedPubTopic>
         <motorPositionPubTopic>/tilt_motor_${motor_number}/position</motorPositionPubTopic>
         <joint_control_pid>
-            <p>1.0</p>
-            <i>0.0</i>
-            <d>0.01</d>
+            <p>${p_gain}</p>
+            <i>${i_gain}</i>
+            <d>${d_gain}</d>
             <iMax>1.0</iMax>
             <iMin>-1.0</iMin>
             <cmdMax>2.0</cmdMax>
@@ -166,6 +166,7 @@
         <momentConstant>${moment_constant}</momentConstant>
         <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
         <motorNumber>${motor_number}</motorNumber>
+        <motorType>velocity</motorType>
         <rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
         <rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
         <motorSpeedPubTopic>/motor_speed/${motor_number}</motorSpeedPubTopic>
@@ -179,7 +180,7 @@
 
   <!-- Double tilt unit joint and link -->
   <xacro:macro name="double_tilt_unit"
-    params="robot_namespace suffix direction_upper direction_lower motor_constant motor_constant_lower moment_constant moment_constant_lower parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit *origin *inertia">
+    params="robot_namespace suffix direction_upper direction_lower motor_constant motor_constant_lower moment_constant moment_constant_lower parent mass_rotor mass_tilt_unit time_constant_up time_constant_down max_rot_velocity rotor_offset_top motor_number rotor_drag_coefficient rolling_moment_coefficient color mesh mesh_tilt_unit p_gain i_gain d_gain *origin *inertia">
 
     <!-- Rotor tilt_unit -->
     <joint name="${robot_namespace}/tilt_unit_${motor_number}_revolutejoint" type="continuous">
@@ -219,9 +220,9 @@
         <motorSpeedPubTopic>/tilt_motor_${motor_number}/velocity</motorSpeedPubTopic>
         <motorPositionPubTopic>/tilt_motor_${motor_number}/position</motorPositionPubTopic>
         <joint_control_pid>
-            <p>1.0</p>
-            <i>0.0</i>
-            <d>0.01</d>
+            <p>${p_gain}</p>
+            <i>${i_gain}</i>
+            <d>${d_gain}</d>
             <iMax>1.0</iMax>
             <iMin>-1.0</iMin>
             <cmdMax>2.0</cmdMax>

rotors_gazebo_plugins/CMakeLists.txt

@@ -4,6 +4,7 @@
 # BUILD_MAVLINK_INTERFACE_PLUGIN    bool    Build mavlink_interface_plugin if mavlink found (requires mav dependency).
 # BUILD_OCTOMAP_PLUGIN              bool    Build the optical map plugin (requires Octomap).
 # BUILD_OPTICAL_FLOW_PLUGIN         bool    Build the optical flow plugin (requires OpenCV).
+# BUILD_AERIAL_MANIPULATION_PLUGINS bool    Build plugins related to aerial manipulation
 # MAVLINK_HEADER_DIR                string  Location of MAVLink header files. If not provided, this CMakeLists.txt file will
 #                                               search the default locations (e.g. ROS) for them. This variable is only required
 #                                               if BUILD_MAVLINK_INTERFACE_PLUGIN=TRUE.
@@ -28,6 +29,11 @@ if(NOT DEFINED BUILD_OCTOMAP_PLUGIN)
   set(BUILD_OCTOMAP_PLUGIN FALSE)
 endif()
 
+if(NOT DEFINED BUILD_AERIAL_MANIPULATION_PLUGINS)
+  message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS variable not provided, setting to TRUE.")
+  set(BUILD_AERIAL_MANIPULATION_PLUGINS TRUE)
+endif()
+
 if(NOT DEFINED BUILD_OPTICAL_FLOW_PLUGIN)
   message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN variable not provided, setting to FALSE.")
   set(BUILD_OPTICAL_FLOW_PLUGIN FALSE)
@@ -84,6 +90,12 @@ else ()
   message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN = FALSE, NOT building gazebo_optical_flow_plugin.")
 endif ()
 
+if(BUILD_AERIAL_MANIPULATION_PLUGINS)
+  message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = TRUE, building aerial manipulation plugins.")
+else ()
+  message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = FALSE, NOT building aerial manipulation plugins.")
+endif ()
+
 if(NO_ROS)
   message(STATUS "NO_ROS = TRUE, building rotors_gazebo_plugins WITHOUT any ROS dependancies.")
 else()
@@ -456,6 +468,18 @@ if(BUILD_OCTOMAP_PLUGIN)
   endif()
   list(APPEND targets_to_install rotors_gazebo_octomap_plugin)
 endif()
+#====================================== Aerial Manipulation PLUGINs ==========================================//
+
+if(BUILD_AERIAL_MANIPULATION_PLUGINS)
+
+  add_library(rotors_gazebo_force_sensor_plugin SHARED src/gazebo_force_sensor_plugin.cpp)
+  target_link_libraries(rotors_gazebo_force_sensor_plugin ${target_linking_LIBRARIES})
+  if (NOT NO_ROS)
+    add_dependencies(rotors_gazebo_force_sensor_plugin ${catkin_EXPORTED_TARGETS})
+  endif()
+  list(APPEND targets_to_install rotors_gazebo_force_sensor_plugin)
+
+endif()
 
 #======================================= ODOMETRY PLUGIN ========================================//
 if (NOT NO_ROS)