model.sdf.erb

<%
if !defined?(name) || name== nil || name.empty?
  $model_name = 'mbzirc_quadrotor'
else
  $model_name = name
end

if defined?(gripper)
  $gripper_model = gripper
end

$attitude_gain = '2 3 0.15'
$motor_constant = '8.54858e-06'
$thrust_with_payload_mass = '0'
if defined?($gripper_model)
  if $gripper_model.include?('mbzirc_oberon7_gripper')
    $attitude_gain = '10 10 0.15'
    $motor_constant = '3.95e-05'
  elsif $gripper_model.include?('mbzirc_suction_gripper')
    $attitude_gain = '10 10 0.15'
    $motor_constant = '2.52e-05'
  end
  if $gripper_model.include?('_light')
    $thrust_with_payload_mass = '1'
  end
end

$slot0_payload = nil
$slot0_rpy = '0 0 0'
$slot1_payload = nil
$slot1_rpy = '0 0 0'
$slot2_payload = nil
$slot2_rpy = '0 0 0'
$slot3_payload = nil
$slot3_rpy = '0 0 0'
$slot4_payload = nil
$slot4_rpy = '0 0 0'
$slot5_payload = nil
$slot5_rpy = '0 0 0'
$slot6_payload = nil
$slot6_rpy = '0 0 0'
$slot7_payload = nil
$slot7_rpy = '0 0 0'

if defined?(slot0)
  $slot0_payload = slot0
end
if defined?(slot0_pos)
  $slot0_rpy = slot0_pos
end
if defined?(slot1)
  $slot1_payload = slot1
end
if defined?(slot1_pos)
  $slot1_rpy = slot1_pos
end
if defined?(slot2)
  $slot2_payload = slot2
end
if defined?(slot2_pos)
  $slot2_rpy = slot2_pos
end
if defined?(slot3)
  $slot3_payload = slot3
end
if defined?(slot3_pos)
  $slot3_rpy = slot3_pos
end
if defined?(slot4)
  $slot4_payload = slot4
end
if defined?(slot4_pos)
  $slot4_rpy = slot4_pos
end
if defined?(slot5)
  $slot5_payload = slot5
end
if defined?(slot5_pos)
  $slot5_rpy = slot5_pos
end
if defined?(slot6)
  $slot6_payload = slot6
end
if defined?(slot6_pos)
  $slot6_rpy = slot6_pos
end
if defined?(slot7)
  $slot7_payload = slot7
end
if defined?(slot7_pos)
  $slot7_rpy = slot7_pos
end
%>

<?xml version="1.0"?>
<sdf version='1.9'>
  <model name="<%= $model_name%>">
    <static>false</static>

    <!-- Platform base model-->
    <include merge="true">
      <uri>model://mbzirc_quadrotor_base</uri>
    </include>

    <% if $gripper_model != nil%>
    <include>
      <name>gripper</name>
      <uri>model://<%= $gripper_model%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="gripper_mount_point">0 0 0 0 0 0</pose>
    </include>
    <joint name="gripper_joint" type="fixed">
      <parent>gripper_mount_point</parent>
      <child>gripper::mount_point</child>
    </joint>
    <% end %>

    <% if $slot0_payload != nil%>
    <!-- Payload slot0 -->
    <include>
      <name>sensor_0</name>
      <uri>model://sensors/<%= $slot0_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_0" degrees="true">
        0 0 0 <%= $slot0_rpy%>
      </pose>
    </include>

    <joint name="slot_0_joint" type="fixed">
      <parent>slot_0</parent>
      <child>sensor_0::mount_point</child>
    </joint>
    <% end %>
    <% if $slot1_payload != nil%>
    <!-- Payload slot1 -->
    <include>
      <name>sensor_1</name>
      <uri>model://sensors/<%= $slot1_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_1" degrees="true">
        0 0 0 <%= $slot1_rpy%>
      </pose>
    </include>

    <joint name="slot_1_joint" type="fixed">
      <parent>slot_1</parent>
      <child>sensor_1::mount_point</child>
    </joint>
    <% end %>
    <% if $slot2_payload != nil%>
    <!-- Payload slot2 -->
    <include>
      <name>sensor_2</name>
      <uri>model://sensors/<%= $slot2_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_2" degrees="true">
        0 0 0 <%= $slot2_rpy%>
      </pose>
    </include>

    <joint name="slot_2_joint" type="fixed">
      <parent>slot_2</parent>
      <child>sensor_2::mount_point</child>
    </joint>
    <% end %>
    <% if $slot3_payload != nil%>
    <!-- Payload slot3 -->
    <include>
      <name>sensor_3</name>
      <uri>model://sensors/<%= $slot3_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_3" degrees="true">
        0 0 0 <%= $slot3_rpy%>
      </pose>
    </include>

    <joint name="slot_3_joint" type="fixed">
      <parent>slot_3</parent>
      <child>sensor_3::mount_point</child>
    </joint>
    <% end %>
    <% if $slot4_payload != nil%>
    <!-- Payload slot4 -->
    <include>
      <name>sensor_4</name>
      <uri>model://sensors/<%= $slot4_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_4" degrees="true">
        0 0 0 <%= $slot4_rpy%>
      </pose>
    </include>

    <joint name="slot_4_joint" type="fixed">
      <parent>slot_4</parent>
      <child>sensor_4::mount_point</child>
    </joint>
    <% end %>
    <% if $slot5_payload != nil%>
    <!-- Payload slot5 -->
    <include>
      <name>sensor_5</name>
      <uri>model://sensors/<%= $slot5_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_5" degrees="true">
        0 0 0 <%= $slot5_rpy%>
      </pose>
    </include>

    <joint name="slot_5_joint" type="fixed">
      <parent>slot_5</parent>
      <child>sensor_5::mount_point</child>
    </joint>
    <% end %>
    <% if $slot6_payload != nil%>
    <!-- Payload slot6 -->
    <include>
      <name>sensor_6</name>
      <uri>model://sensors/<%= $slot6_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_6" degrees="true">
        0 0 0 <%= $slot6_rpy%>
      </pose>
    </include>

    <joint name="slot_6_joint" type="fixed">
      <parent>slot_6</parent>
      <child>sensor_6::mount_point</child>
    </joint>
    <% end %>
    <% if $slot7_payload != nil%>
    <!-- Payload slot7 -->
    <include>
      <name>sensor_7</name>
      <uri>model://sensors/<%= $slot7_payload%></uri>
      <placement_frame>mount_point</placement_frame>
      <pose relative_to="slot_7" degrees="true">
        0 0 0 <%= $slot7_rpy%>
      </pose>
    </include>

    <joint name="slot_7_joint" type="fixed">
      <parent>slot_7</parent>
      <child>sensor_7::mount_point</child>
    </joint>
    <% end %>
    <plugin filename="libignition-gazebo-pose-publisher-system.so"
      name="ignition::gazebo::systems::PosePublisher">
      <publish_link_pose>true</publish_link_pose>
      <publish_sensor_pose>true</publish_sensor_pose>
      <publish_collision_pose>false</publish_collision_pose>
      <publish_visual_pose>false</publish_visual_pose>
      <publish_nested_model_pose>true</publish_nested_model_pose>
      <publish_model_pose>false</publish_model_pose>
      <use_pose_vector_msg>true</use_pose_vector_msg>
      <static_publisher>true</static_publisher>
      <static_update_frequency>1</static_update_frequency>
    </plugin>
    <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
      name="ignition::gazebo::systems::MulticopterMotorModel">
      <robotNamespace>model/<%= $model_name%></robotNamespace>
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>800.0</maxRotVelocity>
      <motorConstant><%= $motor_constant %></motorConstant>
      <momentConstant>0.016</momentConstant>
      <commandSubTopic>command/motor_speed</commandSubTopic>
      <motorNumber>0</motorNumber>
      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
    <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
      name="ignition::gazebo::systems::MulticopterMotorModel">
      <robotNamespace>model/<%= $model_name%></robotNamespace>
      <jointName>rotor_1_joint</jointName>
      <linkName>rotor_1</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>800.0</maxRotVelocity>
      <motorConstant><%= $motor_constant %></motorConstant>
      <momentConstant>0.016</momentConstant>
      <commandSubTopic>command/motor_speed</commandSubTopic>
      <motorNumber>1</motorNumber>
      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
    <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
      name="ignition::gazebo::systems::MulticopterMotorModel">
      <robotNamespace>model/<%= $model_name%></robotNamespace>
      <jointName>rotor_2_joint</jointName>
      <linkName>rotor_2</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>800.0</maxRotVelocity>
      <motorConstant><%= $motor_constant %></motorConstant>
      <momentConstant>0.016</momentConstant>
      <commandSubTopic>command/motor_speed</commandSubTopic>
      <motorNumber>2</motorNumber>
      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
    <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
      name="ignition::gazebo::systems::MulticopterMotorModel">
      <robotNamespace>model/<%= $model_name%></robotNamespace>
      <jointName>rotor_3_joint</jointName>
      <linkName>rotor_3</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>800.0</maxRotVelocity>
      <motorConstant><%= $motor_constant %></motorConstant>
      <momentConstant>0.016</momentConstant>
      <commandSubTopic>command/motor_speed</commandSubTopic>
      <motorNumber>3</motorNumber>
      <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
      <motorType>velocity</motorType>
    </plugin>
    <!--Multicopter velocity controller-->
    <plugin
      filename="libMulticopterControl.so"
      name="ignition::gazebo::systems::MulticopterVelocityControl">
      <robotNamespace>model/<%= $model_name%></robotNamespace>
      <commandSubTopic>cmd_vel</commandSubTopic>
      <motorControlPubTopic>command/motor_speed</motorControlPubTopic>
      <enableSubTopic>velocity_controller/enable</enableSubTopic>
      <comLinkName>base_link</comLinkName>
      <velocityGain>2.7 2.7 2.7</velocityGain>
      <attitudeGain><%= $attitude_gain %></attitudeGain>
      <angularRateGain>0.4 0.52 0.18</angularRateGain>
      <maximumLinearAcceleration>1 1 2</maximumLinearAcceleration>
      <maximumLinearVelocity>5 5 5</maximumLinearVelocity>
      <maximumAngularVelocity>3 3 3</maximumAngularVelocity>
      <linearVelocityNoiseMean>0 0 0.05</linearVelocityNoiseMean>
      <!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
      <!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
      <!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
      <linearVelocityNoiseStdDev>0.1105 0.1261 0.00947</linearVelocityNoiseStdDev>
      <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
      <!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample  -->
      <!-- time of 0.004 ms. -->
      <angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>
      <rotorConfiguration>
        <rotor>
          <jointName>rotor_0_joint</jointName>
          <forceConstant><%= $motor_constant %></forceConstant>
          <momentConstant>0.016</momentConstant>
          <direction>1</direction>
        </rotor>
        <rotor>
          <jointName>rotor_1_joint</jointName>
          <forceConstant><%= $motor_constant %></forceConstant>
          <momentConstant>0.016</momentConstant>
          <direction>1</direction>
        </rotor>
        <rotor>
          <jointName>rotor_2_joint</jointName>
          <forceConstant><%= $motor_constant %></forceConstant>
          <momentConstant>0.016</momentConstant>
          <direction>-1</direction>
        </rotor>
        <rotor>
          <jointName>rotor_3_joint</jointName>
          <forceConstant><%= $motor_constant %></forceConstant>
          <momentConstant>0.016</momentConstant>
          <direction>-1</direction>
        </rotor>
      </rotorConfiguration>
      <thrustWithPayloadMass><%= $thrust_with_payload_mass %></thrustWithPayloadMass>
    </plugin>
    <!-- Battery plugin -->
    <!-- Since we are interested in using time as the limiting factor -->
    <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
      name="ignition::gazebo::systems::LinearBatteryPlugin">
      <battery_name>linear_battery</battery_name>
      <voltage>12.694</voltage>
      <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
      <open_circuit_voltage_linear_coef>0</open_circuit_voltage_linear_coef>
      <initial_charge><%= capacity%></initial_charge>
      <capacity><%= capacity%></capacity>
      <resistance>0.061523</resistance>
      <smooth_current_tau>1.9499</smooth_current_tau>
      <power_load>6.6</power_load>
      <start_on_motion>true</start_on_motion>
      <fix_issue_225>true</fix_issue_225>
    </plugin>

    <!-- Comms endpoint-->
    <plugin
      filename="ignition-gazebo-comms-endpoint-system"
      name="ignition::gazebo::systems::CommsEndpoint">
      <address><%= $model_name%></address>
      <topic>model/<%= $model_name%>/rx</topic>
    </plugin>
 </model>
</sdf>