Robotics StackExchange | Archived questions

Quadcopter made up of cylinders/fixed joints causes IMU problems in gazebo9 with lib gazebo_imu_plugin

I understand this may be a px4 issue and I'll post it to their forums, but I'm wondering if anyone knows what I am doing wrong.

I am working on a quadcopter model. Initially I modified the 3DR Solo model to just change the mesh, inertia, and rotor joints to match my geometry. The aircraft flies fine (other than poorly tuned controller gains).

I wanted the motors modelled, since they currently aren't in my mesh so I added in 4 cylinders attached to the base with fixed joints. When I do this I get a bunch of IMU problems and the accelerometers reading incorrect values.

> pxh> INFO  [mavlink] partner IP: 127.0.0.1
INFO  [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
[Wrn] [GuiIface.cc:120] Failed to get QCocoaScreen for NSObject(0x0)
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/quad_x_custom/motor_speed/3, deleting message. This warning is printed only once.
WARN  [commander] Preflight Fail: Accel Range, hold still on arming
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/quad_x_custom/motor_speed/1, deleting message. This warning is printed only once.
INFO  [ecl/EKF] EKF GPS checks passed (WGS-84 origin set)
INFO  [ecl/EKF] EKF commencing GPS fusion
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/quad_x_custom/motors, deleting message. This warning is printed only once.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/quad_x_custom/motor_speed/2, deleting message. This warning is printed only once.
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
WARN  [ecl/EKF] EKF baro hgt timeout - reset to baro
px4>sensors status
INFO  [sensors] gyro status:
INFO  [ecl/validation] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO  [ecl/validation] sensor #0, prio: 100, state: OK
INFO  [ecl/validation]  val:  -0.0238, lp:  -0.0377 mean dev:   0.0000 RMS:   0.0086 conf:   1.0000
INFO  [ecl/validation]  val:  -0.0310, lp:  -0.0137 mean dev:   0.0001 RMS:   0.0084 conf:   1.0000
INFO  [ecl/validation]  val:  -0.0096, lp:  -0.0122 mean dev:  -0.0001 RMS:   0.0083 conf:   1.0000
INFO  [sensors] accel status:
INFO  [ecl/validation] validator: best: 0, prev best: 0, failsafe: NO (0 events)
INFO  [ecl/validation] sensor #0, prio: 100, state: OK
INFO  [ecl/validation]  val:  -0.0478, lp:   0.0485 mean dev:   0.0037 RMS:   0.1045 conf:   1.0000
INFO  [ecl/validation]  val:  -2.4719, lp:  -2.5461 mean dev:   0.0070 RMS:   0.1087 conf:   1.0000
INFO  [ecl/validation]  val:  -2.4057, lp:  -2.4644 mean dev:   0.1297 RMS:   0.9791 conf:   1.0000

Here is the model that works:

<sdf version='1.5'>
  <model name='quad_x_custom'>
    <!-- quad body -->
    <link name='base_link'>
      <pose>0 0 0 0 0 1.57</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.320</mass>
        <inertia>
          <ixx>0.000785</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00101</iyy>
          <iyz>0</iyz>
          <izz>0.001779</izz>
        </inertia>
      </inertial>
      <collision name='base_link_inertia_collision'>
        <pose>0 0 -0.04 0 0 0</pose>
        <geometry>
          <box>
            <size>0.32 0.32 0.18</size>
          </box>
        </geometry>
        <surface>
          <contact>
            <ode>
              <min_depth>0.001</min_depth>
              <max_vel>0</max_vel>
            </ode>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='base_link_inertia_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/quad_x_custom.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <link name='imu_link'>
      <pose>0 0 0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.015</mass>
        <inertia>
          <ixx>1e-05</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1e-05</iyy>
          <iyz>0</iyz>
          <izz>1e-05</izz>
        </inertia>
      </inertial>
    </link>
    <joint name='imu_joint' type='revolute'>
      <child>imu_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>1 0 0</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='battery'>
      <inertial>
        <mass>.360</mass>
        <inertia>
          <ixx>0.0002041</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0009354</iyy>
          <iyz>0</iyz>
          <izz>0.0009354</izz>
        </inertia>
        <pose>0 -0 0 0 0 0</pose>
      </inertial>
      <pose>0 -0 -0.0275 0 -0 0</pose>
      <gravity>1</gravity>
      <self_collide>0</self_collide>
      <kinematic>0</kinematic>
      <enable_wind>0</enable_wind>
      <visual name='visual'>
        <pose>0 -0 0 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.1 0.035 0.035</size>
          </box>
        </geometry>
        <material>
          <lighting>1</lighting>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Blue</name>
          </script>
          <shader type='pixel'>
            <normal_map></normal_map>
          </shader>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>0.01 0.01 0.01 1</specular>
          <emissive>0 0 0 1</emissive>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose>0 -0 0 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.1 0.035 0.035</size>
          </box>
        </geometry>
      </collision>
    </link>
    <joint name='body_battery_joint' type='fixed'>
      <parent>base_link</parent>
      <child>battery</child>
      <pose>0 0 -0.0275 0 -0 0</pose>
    </joint>
    <link name='rotor_0'>
      <pose>0.1 -0.1 0.02 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_0_collision'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.0635</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_0_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_0_joint' type='revolute'>
      <child>rotor_0</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_1'>
      <pose>-0.1 0.1 0.02 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_1_collision'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.0635</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_1_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_1_joint' type='revolute'>
      <child>rotor_1</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_2'>
      <pose>0.1 0.1 0.02 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_2_collision'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.0635</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_2_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_2_joint' type='revolute'>
      <child>rotor_2</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='rotor_3'>
      <pose>-0.1 -0.1 0.02 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <collision name='rotor_3_collision'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.0635</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_3_visual'>
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_3_joint' type='revolute'>
      <child>rotor_3</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
      <robotNamespace></robotNamespace>
      <linkName>base_link</linkName>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace></robotNamespace>
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>0</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace></robotNamespace>
      <jointName>rotor_1_joint</jointName>
      <linkName>rotor_1</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>1</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='front_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace></robotNamespace>
      <jointName>rotor_2_joint</jointName>
      <linkName>rotor_2</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>2</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='back_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace></robotNamespace>
      <jointName>rotor_3_joint</jointName>
      <linkName>rotor_3</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>3</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
        <robotNamespace></robotNamespace>
        <gpsNoise>true</gpsNoise>
    </plugin>
        <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace/>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <serialEnabled>false</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <hil_mode>false</hil_mode>
      <hil_state_level>false</hil_state_level>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
      <control_channels>
        <channel name='rotor1'>
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor2'>
          <input_index>1</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor3'>
          <input_index>2</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor4'>
          <input_index>3</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
      </control_channels>
    </plugin>
    <static>0</static>
    <plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
      <robotNamespace></robotNamespace>
      <linkName>imu_link</linkName>
      <imuTopic>/imu</imuTopic>
      <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
      <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
      <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
      <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
      <accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
      <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
      <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
      <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
    </plugin>
  </model>
</sdf>

When I add "motors" as fixed joints it doesn't work:

<?xml version='1.0'?>
<sdf version='1.6'>
  <model name='quad_x_custom'>

    <static>0</static>
    <allow_auto_disable>1</allow_auto_disable>

    <link name='base_link'>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.32</mass>
        <inertia>
          <ixx>0.000785</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00101</iyy>
          <iyz>0</iyz>
          <izz>0.001779</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <visual name='base_link_inertia_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/quad_x_custom.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
          <shader type='pixel'/>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='base_link_inertia_collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 -0.04 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.32 0.32 0.18</size>
          </box>
        </geometry>
      </collision>
    </link>
    <link name='battery'>
      <inertial>
        <mass>0.36</mass>
        <inertia>
          <ixx>0.0002041</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0009354</iyy>
          <iyz>0</iyz>
          <izz>0.0009354</izz>
        </inertia>
        <pose frame=''>0 -0 0 0 -0 0</pose>
      </inertial>
      <pose frame=''>-0 0 -0.0275 0 -0 0</pose>
      <gravity>1</gravity>
      <self_collide>0</self_collide>
      <kinematic>0</kinematic>
      <enable_wind>0</enable_wind>
      <visual name='visual'>
        <pose frame=''>0 -0 0 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.1 0.035 0.035</size>
          </box>
        </geometry>
        <material>
          <lighting>1</lighting>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Blue</name>
          </script>
          <shader type='pixel'>
            <normal_map>__default__</normal_map>
          </shader>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>0.01 0.01 0.01 1</specular>
          <emissive>0 0 0 1</emissive>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.1 0.035 0.035</size>
          </box>
        </geometry>
      </collision>
    </link>
    <joint name='body_battery_joint' type='fixed'>
      <parent>base_link</parent>
      <child>battery</child>
      <pose frame=''>0 0 -0.0275 0 -0 0</pose>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>

    <link name='imu_link'>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.015</mass>
        <inertia>
          <ixx>1e-05</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>1e-05</iyy>
          <iyz>0</iyz>
          <izz>1e-05</izz>
        </inertia>
      </inertial>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
    </link>
    <joint name='imu_joint' type='revolute'>
      <parent>base_link</parent>
      <child>imu_link</child>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <axis>
        <xyz>1 0 0</xyz>
        <use_parent_model_frame>1</use_parent_model_frame>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>

    <link name='motor_0'>
      <inertial>
        <mass>0.07</mass>
        <inertia>
          <ixx>0.00013125</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00013125</iyy>
          <iyz>0</iyz>
          <izz>0.0001125</izz>
        </inertia>
        <pose>0 0 0 0 -0 0</pose>
      </inertial>
      <pose>0.1 -0.1 -0 0 -0 0</pose>
      <gravity>1</gravity>
      <self_collide>0</self_collide>
      <kinematic>0</kinematic>
      <enable_wind>0</enable_wind>
      <visual name='visual'>
        <pose>0 0 0.015 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.013</radius>
            <length>0.015</length>
          </cylinder>
        </geometry>
        <material>
          <lighting>1</lighting>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
          <shader type='pixel'>
            <normal_map>__default__</normal_map>
          </shader>
          <ambient>0.3 0.3 0.3 1</ambient>
          <diffuse>0.7 0.7 0.7 1</diffuse>
          <specular>0.01 0.01 0.01 1</specular>
          <emissive>0 0 0 1</emissive>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.013</radius>
            <length>0.015</length>
          </cylinder>
        </geometry>
      </collision>
    </link>
    <joint name='base_motor_0_joint' type='fixed'>
      <parent>base_link</parent>
      <child>motor_0</child>
      <pose frame=''>0.1 -0.1 0.02 0 -0 0</pose>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <link name='motor_1'>
          <inertial>
            <mass>0.07</mass>
            <inertia>
              <ixx>0.00013125</ixx>
              <ixy>0</ixy>
              <ixz>0</ixz>
              <iyy>0.00013125</iyy>
              <iyz>0</iyz>
              <izz>0.0001125</izz>
            </inertia>
            <pose>0 0 0 0 -0 0</pose>
          </inertial>
          <pose>-0.1 0.1 -0 0 -0 0</pose>
          <gravity>1</gravity>
          <self_collide>0</self_collide>
          <kinematic>0</kinematic>
          <enable_wind>0</enable_wind>
          <visual name='visual'>
            <pose frame=''>0 0 0.015 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.013</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
            <material>
              <lighting>1</lighting>
              <script>
                <uri>file://media/materials/scripts/gazebo.material</uri>
                <name>Gazebo/Grey</name>
              </script>
              <shader type='pixel'>
                <normal_map>__default__</normal_map>
              </shader>
              <ambient>0.3 0.3 0.3 1</ambient>
              <diffuse>0.7 0.7 0.7 1</diffuse>
              <specular>0.01 0.01 0.01 1</specular>
              <emissive>0 0 0 1</emissive>
            </material>
            <transparency>0</transparency>
            <cast_shadows>1</cast_shadows>
          </visual>
          <collision name='collision'>
            <laser_retro>0</laser_retro>
            <max_contacts>10</max_contacts>
            <pose>0 0 0 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.013</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
          </collision>
    </link>
    <joint name='base_motor_1_joint' type='fixed'>
      <parent>base_link</parent>
      <child>motor_1</child>
      <pose frame=''>-0.1 0.1 0.02 0 -0 0</pose>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <link name='motor_2'>
          <inertial>
            <mass>0.07</mass>
            <inertia>
              <ixx>0.00013125</ixx>
              <ixy>0</ixy>
              <ixz>0</ixz>
              <iyy>0.00013125</iyy>
              <iyz>0</iyz>
              <izz>0.0001125</izz>
            </inertia>
            <pose>0 0 0 0 -0 0</pose>
          </inertial>
          <pose>0.1 0.1 -0 0 -0 0</pose>
          <gravity>1</gravity>
          <self_collide>0</self_collide>
          <kinematic>0</kinematic>
          <enable_wind>0</enable_wind>
          <visual name='visual'>
            <pose frame=''>0 0 0.015 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.013</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
            <material>
              <lighting>1</lighting>
              <script>
                <uri>file://media/materials/scripts/gazebo.material</uri>
                <name>Gazebo/Grey</name>
              </script>
              <shader type='pixel'>
                <normal_map>__default__</normal_map>
              </shader>
              <ambient>0.3 0.3 0.3 1</ambient>
              <diffuse>0.7 0.7 0.7 1</diffuse>
              <specular>0.01 0.01 0.01 1</specular>
              <emissive>0 0 0 1</emissive>
            </material>
            <transparency>0</transparency>
            <cast_shadows>1</cast_shadows>
          </visual>
          <collision name='collision'>
            <laser_retro>0</laser_retro>
            <max_contacts>10</max_contacts>
            <pose>0 0 0 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.012</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
          </collision>
    </link>
    <joint name='base_motor_2_joint' type='fixed'>
      <parent>base_link</parent>
      <child>motor_2</child>
      <pose frame=''>0.1 0.1 0.02 0 -0 0</pose>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <link name='motor_3'>
          <inertial>
            <mass>0.07</mass>
            <inertia>
              <ixx>0.00013125</ixx>
              <ixy>0</ixy>
              <ixz>0</ixz>
              <iyy>0.00013125</iyy>
              <iyz>0</iyz>
              <izz>0.0001125</izz>
            </inertia>
            <pose>0 0 0 0 -0 0</pose>
          </inertial>
          <pose>-0.1 -0.1 -0 0 -0 0</pose>
          <gravity>1</gravity>
          <self_collide>0</self_collide>
          <kinematic>0</kinematic>
          <enable_wind>0</enable_wind>
          <visual name='visual'>
            <pose frame=''>0 0 0.015 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.013</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
            <material>
              <lighting>1</lighting>
              <script>
                <uri>file://media/materials/scripts/gazebo.material</uri>
                <name>Gazebo/Grey</name>
              </script>
              <shader type='pixel'>
                <normal_map>__default__</normal_map>
              </shader>
              <ambient>0.3 0.3 0.3 1</ambient>
              <diffuse>0.7 0.7 0.7 1</diffuse>
              <specular>0.01 0.01 0.01 1</specular>
              <emissive>0 0 0 1</emissive>
            </material>
            <transparency>0</transparency>
            <cast_shadows>1</cast_shadows>
          </visual>
          <collision name='collision'>
            <laser_retro>0</laser_retro>
            <max_contacts>10</max_contacts>
            <pose>0 0 0 0 -0 0</pose>
            <geometry>
              <cylinder>
                <radius>0.013</radius>
                <length>0.015</length>
              </cylinder>
            </geometry>
          </collision>
    </link>
    <joint name='base_motor_3_joint' type='fixed'>
      <parent>base_link</parent>
      <child>motor_3</child>
      <pose frame=''>-0.1 -0.1 0.02 0 -0 0</pose>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>     

    <link name='rotor_0'>
      <pose frame=''>0.1 -0.1 0.025 0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <visual name='rotor_0_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
          <shader type='pixel'/>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='rotor_0_collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.0635</radius>
            <length>0.005</length>
          </cylinder>
        </geometry>
      </collision>
    </link>
    <link name='rotor_1'>
      <pose frame=''>-0.1 0.1 0.025 0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <visual name='rotor_1_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
          <shader type='pixel'/>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='rotor_1_collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.0635</radius>
            <length>0.005</length>
          </cylinder>
        </geometry>
      </collision>
    </link>
    <link name='rotor_2'>
      <pose frame=''>0.1 0.1 0.025 0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <visual name='rotor_2_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
          <shader type='pixel'/>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='rotor_2_collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.0635</radius>
            <length>0.005</length>
          </cylinder>
        </geometry>
      </collision>
    </link>
    <link name='rotor_3'>
      <pose frame=''>-0.1 -0.1 0.025  0 -0 0</pose>
      <inertial>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <mass>0.005</mass>
        <inertia>
          <ixx>9.75e-07</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000273104</iyy>
          <iyz>0</iyz>
          <izz>0.000274004</izz>
        </inertia>
      </inertial>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
      <enable_wind>0</enable_wind>
      <kinematic>0</kinematic>
      <visual name='rotor_3_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://quad_x_custom/meshes/prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
          <shader type='pixel'/>
        </material>
        <transparency>0</transparency>
        <cast_shadows>1</cast_shadows>
      </visual>
      <collision name='rotor_3_collision'>
        <laser_retro>0</laser_retro>
        <max_contacts>10</max_contacts>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.0635</radius>
            <length>0.005</length>
          </cylinder>
        </geometry>
      </collision>
    </link>

    <joint name='rotor_0_joint' type='revolute'>
      <parent>motor_0</parent>
      <child>rotor_0</child>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <use_parent_model_frame>1</use_parent_model_frame>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
          <effort>-1</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <joint name='rotor_1_joint' type='revolute'>
      <parent>motor_1</parent>
      <child>rotor_1</child>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <use_parent_model_frame>1</use_parent_model_frame>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
          <effort>-1</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <joint name='rotor_2_joint' type='revolute'>
      <parent>motor_2</parent>
      <child>rotor_2</child>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <use_parent_model_frame>1</use_parent_model_frame>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
          <effort>-1</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>
    <joint name='rotor_3_joint' type='revolute'>
      <parent>motor_3</parent>
      <child>rotor_3</child>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <axis>
        <xyz>0 0 1</xyz>
        <use_parent_model_frame>1</use_parent_model_frame>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
          <effort>-1</effort>
          <velocity>-1</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
          <damping>0</damping>
          <friction>0</friction>
        </dynamics>
      </axis>
      <physics>
        <ode>
          <limit>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </limit>
          <suspension>
            <cfm>0</cfm>
            <erp>0.2</erp>
          </suspension>
        </ode>
      </physics>
    </joint>


    <plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>rotor_1_joint</jointName>
      <linkName>rotor_1</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>1</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='back_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>rotor_3_joint</jointName>
      <linkName>rotor_3</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>3</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='front_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>rotor_2_joint</jointName>
      <linkName>rotor_2</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>2</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>rotor_0_joint</jointName>
      <linkName>rotor_0</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1500</maxRotVelocity>
      <motorConstant>8.54858e-06</motorConstant>
      <momentConstant>0.06</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>0</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
      <robotNamespace/>
      <linkName>imu_link</linkName>
      <imuTopic>/imu</imuTopic>
      <gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
      <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
      <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
      <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
      <accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
      <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
      <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
      <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
    </plugin>
    <plugin name='gps_plugin' filename='libgazebo_gps_plugin.so'>
      <robotNamespace/>
      <gpsNoise>1</gpsNoise>
    </plugin>
    <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace/>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <serialEnabled>0</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <hil_mode>0</hil_mode>
      <hil_state_level>0</hil_state_level>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
      <control_channels>
        <channel name='rotor1'>
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor2'>
          <input_index>1</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor3'>
          <input_index>2</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name='rotor4'>
          <input_index>3</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1200</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
      </control_channels>
    </plugin>
    <plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
      <robotNamespace/>
      <linkName>base_link</linkName>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>

  </model>
</sdf>

Asked by variable on 2018-12-14 15:29:45 UTC

Comments

I am working on something similar. Did you solve the problem?

Asked by zaccer on 2019-09-10 07:39:00 UTC

Answers