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