[SOLVED] Custom Quad Gazebo Model Runs with slow Real Time Factor

Hi.

I created a custom quadrotor model in Solidworks and was able to convert tor URDF then to SDF. I hen added the required plugins to work with the PX4 SITL setup (I followed the iris model sdf file) I also changed the motor plugin parameters to adapt to the custom vehicle characteristics. I placed the sdf and .config file inside the Frimware/Tools/sitl_gazebo/models/my_quad/ folder. I am finally able to load it in Gazebo using roslaunh px4 mavros_posix_sitl.launch.

The issue is that the real time factor in Gazebo drops to ~0.6 although I have used simple shapes for collision tags in the sdf file (e.g. cylinders and boxes). I am not sure what the problem could be, so any hints are appreciated.

One thing I noticed is that when I comment out the mavlink interface plugin inside the SDF file, the real time factor becomes solid 1! However, of course, there will be no interface to PX4 SITL without the mavlink interface plugin. So I am not sure if it’s really the bottleneck or there could be another reason.

Thanks.

The following is the SDF file content in case it helps.

<?xml version="1.0" ?>
<sdf version='1.6'>
  <model name='custom_quad'>
    <link name='base_link'>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <inertial>
        <pose frame=''>0.003617 2.4e-05 -0.001015 0 -0 0</pose>
        <mass>3.51896</mass>
        <inertia>
          <ixx>0.0429535</ixx>
          <ixy>8.40732e-05</ixy>
          <ixz>3.76105e-05</ixz>
          <iyy>0.0541371</iyy>
          <iyz>5.6719e-05</iyz>
          <izz>0.0930404</izz>
        </inertia>
      </inertial>
      <collision name='base_link_collision'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.468 0.468 0.13</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_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://swarm/meshes/base_link.STL</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Yellow</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>

      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
 
    </link>

  

    <!-- Moving components -->

    <link name='prop1_link'>
      <pose frame=''>0.2338 -0.22065 0.00871 0 -0 0.33457</pose>
      <inertial>
        <pose frame=''>0 0 0.011562 0 -0 0</pose>
        <mass>0.010691</mass>
        <inertia>
          <ixx>1.4282e-05</ixx>
          <ixy>-2.3637e-05</ixy>
          <ixz>3.691e-22</ixz>
          <iyy>4.2188e-05</iyy>
          <iyz>3.3476e-22</iyz>
          <izz>5.6323e-05</izz>
        </inertia>
      </inertial>
      <collision name='prop1_link_collision'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.01</length>
            <radius>0.1524</radius>
          </cylinder>
        </geometry>
          <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='prop1_link_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://swarm/meshes/prop1_link.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='prop1_joint' type='revolute'>
      <child>prop1_link</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='prop2_link'>
      <pose frame=''>-0.20611 0.21927 0.00871 0 0 -2.5796</pose>
      <inertial>
        <pose frame=''>0 0 0.011562 0 -0 0</pose>
        <mass>0.010691</mass>
        <inertia>
          <ixx>1.4282e-05</ixx>
          <ixy>-2.3637e-05</ixy>
          <ixz>1.764e-22</ixz>
          <iyy>4.2188e-05</iyy>
          <iyz>1.9662e-22</iyz>
          <izz>5.6323e-05</izz>
        </inertia>
      </inertial>
      <collision name='prop2_link_collision'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.01</length>
            <radius>0.1524</radius>
          </cylinder>
        </geometry>
          <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='prop2_link_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://swarm/meshes/prop2_link.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='prop2_joint' type='revolute'>
      <child>prop2_link</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='prop3_link'>
      <pose frame=''>0.2338 0.21927 0.00871 0 0 -1.4608</pose>
      <inertial>
        <pose frame=''>0 0 0.011562 0 -0 0</pose>
        <mass>0.0106912</mass>
        <inertia>
          <ixx>1.4282e-05</ixx>
          <ixy>-2.3637e-05</ixy>
          <ixz>6.43744e-22</ixz>
          <iyy>4.21884e-05</iyy>
          <iyz>1.11232e-22</iyz>
          <izz>5.63234e-05</izz>
        </inertia>
      </inertial>
      <collision name='prop3_link_collision'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.01</length>
            <radius>0.1524</radius>
          </cylinder>
        </geometry>
          <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='prop3_link_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://swarm/meshes/prop3_link.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='prop3_joint' type='revolute'>
      <child>prop3_link</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='prop4_link'>
      <pose frame=''>-0.20611 -0.22065 0.00871 0 0 -1.6378</pose>
      <inertial>
        <pose frame=''>0 -0 0.011562 0 -0 0</pose>
        <mass>0.010691</mass>
        <inertia>
          <ixx>1.4282e-05</ixx>
          <ixy>-2.3637e-05</ixy>
          <ixz>2.3568e-22</ixz>
          <iyy>4.2188e-05</iyy>
          <iyz>2.0636e-22</iyz>
          <izz>5.6323e-05</izz>
        </inertia>
      </inertial>
      <collision name='prop4_link_collision'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.01</length>
            <radius>0.1524</radius>
          </cylinder>
        </geometry>
          <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='prop4_link_visual'>
        <pose frame=''>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://swarm/meshes/prop4_link.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='prop4_joint' type='revolute'>
      <child>prop4_link</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>

    

    <!-- IMU Link. Copied from PX4 Iris model-->
    <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>
    </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>

    <!-- This is copied from Iris model from PX4 -->
    <plugin name='rosbag' filename='libgazebo_multirotor_base_plugin.so'>
      <robotNamespace/>
      <linkName>base_link</linkName>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>

    <!-- ################################ Sensors plugins ################################ -->
    <plugin name='gps_plugin' filename='libgazebo_gps_plugin.so'>
      <robotNamespace/>
      <gpsNoise>1</gpsNoise>
    </plugin>

    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
      <robotNamespace/>
      <pubRate>20</pubRate>
      <noiseDensity>0.0004</noiseDensity>
      <randomWalk>6.4e-06</randomWalk>
      <biasCorrelationTime>600</biasCorrelationTime>
      <magTopic>/mag</magTopic>
    </plugin>

    <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
      <robotNamespace/>
      <pubRate>10</pubRate>
      <baroTopic>/baro</baroTopic>
    </plugin>

    <plugin name='rotors_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>

    <!-- ################################ Motors plugins ################################ -->
    <plugin name='front_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>prop1_joint</jointName>
      <linkName>prop1_link</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>2.67815804739e-05</motorConstant>
      <momentConstant>0.0242552133272</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>0</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-6</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='back_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>prop2_joint</jointName>
      <linkName>prop2_link</linkName>
      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>2.67815804739e-05</motorConstant>
      <momentConstant>0.0242552133272</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>1</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-6</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='front_left_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>prop3_joint</jointName>
      <linkName>prop3_link</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>2.67815804739e-05</motorConstant>
      <momentConstant>0.0242552133272</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>2</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-6</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>
    <plugin name='back_right_motor_model' filename='libgazebo_motor_model.so'>
      <robotNamespace/>
      <jointName>prop4_joint</jointName>
      <linkName>prop4_link</linkName>
      <turningDirection>cw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1100</maxRotVelocity>
      <motorConstant>2.67815804739e-05</motorConstant>
      <momentConstant>0.0242552133272</momentConstant>
      <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
      <motorNumber>3</motorNumber>
      <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
      <rollingMomentCoefficient>1e-6</rollingMomentCoefficient>
      <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
      <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
    </plugin>


    <!-- ################################ MAVLink Plugin ################################ -->
    <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace/>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <magSubTopic>/mag</magSubTopic>
      <baroSubTopic>/baro</baroSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <mavlink_tcp_port>4560</mavlink_tcp_port>
      <serialEnabled>0</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <sdk_addr>INADDR_ANY</sdk_addr>
      <sdk_udp_port>14540</sdk_udp_port>
      <hil_mode>0</hil_mode>
      <hil_state_level>0</hil_state_level>
      <vehicle_is_tailsitter>0</vehicle_is_tailsitter>
      <send_vision_estimation>1</send_vision_estimation>
      <send_odometry>0</send_odometry>
      <enable_lockstep>1</enable_lockstep>
      <use_tcp>true</use_tcp>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
      <control_channels>
        <channel name='rotor1'>
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1000</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>1000</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>1000</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>1000</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>

  </model>
</sdf>


@TSC21 Any possible feedback on this? Thanks.

I actually solved the issue, but the solution is weird to me. The problem was in the order of loading plugins inside the SDF file. With the following order (which is the same as in iris model, it works perfectly fine!
The following plugins are added after all links and joints

  1. libgazebo_multirotor_base_plugin
  2. libgazebo_motor_model (for all motors)
  3. libgazebo_gps_plugin / libgazebo_magnetometer_plugin / libgazebo_barometer_plugin
  4. libgazebo_mavlink_interface
  5. adding <static>0</static> tag after mavlink plugin
  6. libgazebo_imu_plugin

I hope someone can explain why this is the case.

I hope this is helpful!