forked from xtdrone/XTDrone
553 lines
17 KiB
XML
Executable File
553 lines
17 KiB
XML
Executable File
<?xml version="1.0" ?>
|
|
<sdf version='1.5'>
|
|
<model name='solo_stereo_cam'>
|
|
|
|
<!-- Solo body -->
|
|
<link name='base_link'>
|
|
<pose>0 0 0 0 0 0</pose>
|
|
<inertial>
|
|
<pose>0 0 0 0 0 0</pose>
|
|
<mass>1.47</mass>
|
|
<inertia>
|
|
<ixx>0.011</ixx>
|
|
<ixy>0</ixy>
|
|
<ixz>0</ixz>
|
|
<iyy>0.015</iyy>
|
|
<iyz>0</iyz>
|
|
<izz>0.021</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://solo/meshes/solo.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='rotor_0'>
|
|
<pose>0.14745 -0.14525 0.051 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.128</radius>
|
|
</cylinder>
|
|
</geometry>
|
|
<surface>
|
|
<contact>
|
|
<ode/>
|
|
</contact>
|
|
<friction>
|
|
<ode/>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name='rotor_0_visual'>
|
|
<pose>-0.14745 -0.14525 -0.051 0 0 0</pose>
|
|
<geometry>
|
|
<mesh>
|
|
<scale>1 1 1</scale>
|
|
<uri>model://solo/meshes/solo_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.14745 0.14525 0.051 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.128</radius>
|
|
</cylinder>
|
|
</geometry>
|
|
<surface>
|
|
<contact>
|
|
<ode/>
|
|
</contact>
|
|
<friction>
|
|
<ode/>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name='rotor_1_visual'>
|
|
<pose>-0.14745 -0.14525 -0.051 0 0 0</pose>
|
|
<geometry>
|
|
<mesh>
|
|
<scale>1 1 1</scale>
|
|
<uri>model://solo/meshes/solo_prop_ccw.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_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.14745 0.14525 0.051 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.128</radius>
|
|
</cylinder>
|
|
</geometry>
|
|
<surface>
|
|
<contact>
|
|
<ode/>
|
|
</contact>
|
|
<friction>
|
|
<ode/>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name='rotor_2_visual'>
|
|
<pose>-0.14745 0.14525 -0.051 0 0 0</pose>
|
|
<geometry>
|
|
<mesh>
|
|
<scale>1 1 1</scale>
|
|
<uri>model://solo/meshes/solo_prop_cw.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_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.14745 -0.14525 0.051 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.128</radius>
|
|
</cylinder>
|
|
</geometry>
|
|
<surface>
|
|
<contact>
|
|
<ode/>
|
|
</contact>
|
|
<friction>
|
|
<ode/>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name='rotor_3_visual'>
|
|
<pose>-0.14745 0.14525 -0.051 0 0 0</pose>
|
|
<geometry>
|
|
<mesh>
|
|
<scale>1 1 1</scale>
|
|
<uri>model://solo/meshes/solo_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='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
|
|
<robotNamespace/>
|
|
<pubRate>100</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>50</pubRate>
|
|
<baroTopic>/baro</baroTopic>
|
|
</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>
|
|
<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>
|
|
<enable_lockstep>true</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>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>
|
|
|
|
<!-- For Stereo Camera Payload -->
|
|
<include>
|
|
<uri>model://stereo_camera</uri>
|
|
<pose>0 0 -0.05 0 0 0</pose>
|
|
</include>
|
|
|
|
<joint name="stereo_joint" type="fixed">
|
|
<child>stereo_camera::link</child>
|
|
<parent>base_link</parent>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit>
|
|
<upper>0</upper>
|
|
<lower>0</lower>
|
|
</limit>
|
|
</axis>
|
|
</joint>
|
|
<!-- For IMU-->
|
|
<include>
|
|
<uri>model://imu_gazebo</uri>
|
|
<pose>0 0 -0.05 0 0 0</pose>
|
|
</include>
|
|
<joint name="imu_gazebo_joint" type="fixed">
|
|
<child>imu_gazebo::link</child>
|
|
<parent>base_link</parent>
|
|
<axis>
|
|
<xyz>0 0 1</xyz>
|
|
<limit>
|
|
<upper>0</upper>
|
|
<lower>0</lower>
|
|
</limit>
|
|
</axis>
|
|
</joint>
|
|
</model>
|
|
</sdf>
|
|
<!-- vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
|
|
|
|
|
|
|
|
|