ethz-asl · karenbodie · Feb 15, 2021 · Feb 16, 2021 · Mar 1, 2021 · Mar 2, 2021
diff --git a/rotors_description/meshes/omav/omav3_base.stl b/rotors_description/meshes/omav/omav3_base.stl
diff --git a/rotors_description/meshes/omav/omav3_tiltarm.stl b/rotors_description/meshes/omav/omav3_tiltarm.stl
diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro
@@ -312,8 +312,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>
@@ -344,6 +344,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>
@@ -788,7 +789,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>
@@ -809,7 +811,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>
@@ -861,7 +864,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>

diff --git a/rotors_description/urdf/delta_actuators.xacro b/rotors_description/urdf/delta_actuators.xacro
@@ -0,0 +1,102 @@
+<?xml version="1.0"?>
+
+<robot name="${robot_name}" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- Relationship between the p, d gain and the closed-loop dynamics behaviour of the system:
+  It behaves like a second-order system in the frequency domain: Y(s) = U(s)*(ds + p)/(Is**2 + ds + p)
+  Y(s) is the Laplace transform of the actual tilt motor angle and U(s) is the one of the tilt motor angle command, p and d are the gains, I is the inertia of the link.
+  The pid gain set here corresponds to p = 1200, i = 1000, d =500 on the firmware.
+   -->
+
+<xacro:property name="delta_control_mode" value="$(arg delta_control_mode)" />
+<xacro:property name="servo_zero_offset" value="0.0" />
+
+<!-- cmd_min/max = 0 for unlimited -->
+<xacro:property name="servo_cmd_min" value="-100.0" />
+<xacro:property name="servo_cmd_max" value="100.0" />
+<xacro:property name="servo_i_min" value="-20.0" />
+<xacro:property name="servo_i_max" value="20.0" />
+
+<xacro:if value="${delta_control_mode == 'position'}">
+  <xacro:property name="p_gain_delta" value="5.0" />
+  <xacro:property name="i_gain_delta" value="0.1" />
+  <xacro:property name="d_gain_delta" value="1.0" />
+</xacro:if>
+
+<xacro:if value="${delta_control_mode == 'velocity'}">
+  <xacro:property name="p_gain_delta" value="0.1" />
+  <xacro:property name="i_gain_delta" value="0.0" />
+  <xacro:property name="d_gain_delta" value="0.0" />
+</xacro:if>
+
+<!-- effort gains not used, just for macro compatibility -->
+<xacro:if value="${delta_control_mode == 'effort'}">
+  <xacro:property name="p_gain_delta" value="0.5" />
+  <xacro:property name="i_gain_delta" value="0.01" />
+  <xacro:property name="d_gain_delta" value="0.1" />
+</xacro:if>
+
+  <!-- Add motors. -->
+  <gazebo>
+      <plugin name="delta_multimotor_plugin" filename="librotors_gazebo_multimotor_plugin.so">
+        <robotNamespace>${delta_namespace}</robotNamespace>
+        <servos>
+            <servo>
+              <jointName>${delta_namespace}/delta_joint_upper_0</jointName>
+              <spinDirection>cw</spinDirection>
+              <controlMode>${delta_control_mode}</controlMode>
+              <maxRotVelocity>${10}</maxRotVelocity>
+              <minRotPosition>${-pi}</minRotPosition>
+              <maxRotPosition>${pi}</maxRotPosition>
+              <zeroOffset>${servo_zero_offset}</zeroOffset>
+              <joint_control_pid>
+                  <p>${p_gain_delta}</p>
+                  <i>${i_gain_delta}</i>
+                  <d>${d_gain_delta}</d>
+                  <iMax>${servo_i_max}</iMax>
+                  <iMin>${servo_i_min}</iMin>
+                  <cmdMax>${servo_cmd_max}</cmdMax>
+                  <cmdMin>${servo_cmd_min}</cmdMin>
+              </joint_control_pid>
+            </servo>
+            <servo>
+              <jointName>${delta_namespace}/delta_joint_upper_1</jointName>
+              <spinDirection>cw</spinDirection>
+              <controlMode>${delta_control_mode}</controlMode>
+              <maxRotVelocity>${10}</maxRotVelocity>
+              <minRotPosition>${-pi}</minRotPosition>
+              <maxRotPosition>${pi}</maxRotPosition>
+              <zeroOffset>${servo_zero_offset}</zeroOffset>
+              <joint_control_pid>
+                  <p>${p_gain_delta}</p>
+                  <i>${i_gain_delta}</i>
+                  <d>${d_gain_delta}</d>
+                  <iMax>${servo_i_max}</iMax>
+                  <iMin>${servo_i_min}</iMin>
+                  <cmdMax>${servo_cmd_max}</cmdMax>
+                  <cmdMin>${servo_cmd_min}</cmdMin>
+              </joint_control_pid>
+            </servo>
+            <servo>
+              <jointName>${delta_namespace}/delta_joint_upper_2</jointName>
+              <spinDirection>cw</spinDirection>
+              <controlMode>${delta_control_mode}</controlMode>
+              <maxRotVelocity>${10}</maxRotVelocity>
+              <minRotPosition>${-pi}</minRotPosition>
+              <maxRotPosition>${pi}</maxRotPosition>
+              <zeroOffset>${servo_zero_offset}</zeroOffset>
+              <joint_control_pid>
+                  <p>${p_gain_delta}</p>
+                  <i>${i_gain_delta}</i>
+                  <d>${d_gain_delta}</d>
+                  <iMax>${servo_i_max}</iMax>
+                  <iMin>${servo_i_min}</iMin>
+                  <cmdMax>${servo_cmd_max}</cmdMax>
+                  <cmdMin>${servo_cmd_min}</cmdMin>
+              </joint_control_pid>
+            </servo>
+        </servos>
+
+      </plugin>
+  </gazebo>
+
+</robot>
diff --git a/rotors_description/urdf/delta_base.xacro b/rotors_description/urdf/delta_base.xacro
@@ -0,0 +1,103 @@
+<?xml version="1.0"?>
+<!--
+  Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+  Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+  Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+  Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
+  Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+
+  Licensed under the Apache License, Version 2.0 (the "License");
+  you may not use this file except in compliance with the License.
+  You may obtain a copy of the License at
+
+  http://www.apache.org/licenses/LICENSE-2.0
+
+  Unless required by applicable law or agreed to in writing, software
+  distributed under the License is distributed on an "AS IS" BASIS,
+  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  See the License for the specific language governing permissions and
+  limitations under the License.
+-->
+
+<robot name="$(arg mav_name)" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro"/>
+
+  <xacro:property name="namespace" value="$(arg namespace)"/>
+
+ <link name="world"/>
+
+ <joint name="${namespace}/fixed_base_joint" type="fixed">
+      <parent link="world"/>
+      <child link="${namespace}/base_link"/>
+      <axis xyz="1 0 0"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+
+    <!-- Base Link -->
+  <link name="${namespace}/base_link">
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="${0.05} ${0.05} ${0.01}"/>
+      </geometry>
+    </collision>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="${0.05} ${0.05} ${0.01}"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <xacro:property name="robot_name" value="$(arg mav_name)" />
+  <xacro:property name="delta_namespace" value="$(arg namespace)" />
+  <xacro:property name="mounting_link" value="${namespace}/base_link" />
+  <xacro:property name="height_offset" value="0.0" />
+  <xacro:include filename="$(find rotors_description)/urdf/delta_manipulator.xacro" />
+  <xacro:include filename="$(find rotors_description)/urdf/delta_actuators.xacro" />
+
+  <!-- Mount an ADIS16448 IMU. -->
+  <!-- Note that the gyroscope_noise_density is not the discrete noise density used in the simulation. In gazebo_imu_plugin, the discrete noise density is computed as 1/sqrt(dt)*gyroscope_noise_density, where dt the time step in simulation -->
+  <xacro:imu_plugin_macro
+    namespace="${namespace}"
+    imu_suffix=""
+    parent_link="${namespace}/base_link"
+    imu_topic="imu"
+    mass_imu_sensor="0.015"
+    gyroscope_noise_density="0.00005366563"
+    gyroscope_random_walk="0.000038785"
+    gyroscope_bias_correlation_time="1000.0"
+    gyroscope_turn_on_bias_sigma="0.0087"
+    accelerometer_noise_density="0.004"
+    accelerometer_random_walk="0.006"
+    accelerometer_bias_correlation_time="300.0"
+    accelerometer_turn_on_bias_sigma="0.1960"
+    measurement_divisor="2">
+    <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>
+
+  <xacro:if value="$(arg enable_ground_truth)">
+      <xacro:ground_truth_imu_and_odometry
+        namespace="${namespace}"
+        parent_link="${namespace}/base_link"/>
+  </xacro:if>
+
+  <xacro:if value="$(arg enable_logging)">
+      <!-- Instantiate a logger. -->
+      <xacro:bag_plugin_macro
+        namespace="${namespace}"
+        bag_file="$(arg log_file)"
+        rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}"
+        wait_to_record_bag="$(arg wait_to_record_bag)"/>
+  </xacro:if>
+
+</robot>