Thanks to iris.xacro provided, I can add a manipulator under it easily. However, when I want to establish a simulation platform for aerial manipulator with hexrcopter, I need a xacro file for typhoon_h480 but it’s not exist.
I try to build a xacro file according the typhoon_h480.sdf, but it crash.
here is my typhoon_h480.xacro
<?xml version="1.0"?>
<robot name="asix" xmlns:xacro="http://ros.org/wiki/xacro">
<link name='base_link'>
<inertial>
<origin xyz="0.001005 0 -0.0090035" rpy="0 0 0" />
<!--<origin xyz="0.0 0 0" rpy="0 0 0" />-->
<mass value="2.02" />
<inertia ixx="0.011" ixy = "0" ixz = "0"
iyy="0.015" iyz= "0" izz="0.021" />
</inertial>
<collision name='base_link_collision'>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.67 0.67 0.15"/>
</geometry>
</collision>
<visual name='base_link_visual'>
<origin xyz="0 0 0" rpy="0 0 3.141592"/>
<geometry>
<mesh filename="package://mobot_urdf/meshes/main_body_remeshed_v3.stl" scale="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<link name="left_leg">
<inertial>
<origin xyz="0 -0.14314 -0.207252" rpy="0 0 0" />
<mass value="0.1" />
<inertia ixx="0.001" ixy = "0" ixz = "0"
iyy="0.001" iyz= "0" izz="0.001" />
</inertial>
<collision name='collision'>
<origin xyz="-0.005 -0.14314 -0.207252" rpy="0 1.56893 0" />
<geometry>
<cylinder radius="0.012209" length="0.3" />
</geometry>
</collision>
<collision name='collision_bar'>
<origin xyz="0.00052 -0.08503 -0.121187" rpy="-0.501318 0 0" />
<geometry>
<cylinder radius="0.00914984" length="0.176893" />
</geometry>
</collision>
<visual name='base_link_left_leg'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/leg2_remeshed_v3.stl" scale="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<joint name='left_leg_joint' type='fixed'>
<child link="left_leg"/>
<parent link="base_link"/>
<!--<origin xyz="0.00026 -0.040515 -0.048" rpy="0 0 0" /> -->
<origin xyz="0.00026 -0.009 -0.018" rpy="0 0 0" />
<axis xyz="-1 0 0"/>
<!--limit velocity="-1" effort="100" lower="0" upper="1" />
<dynamics damping="0.1"/-->
</joint>
<link name="right_leg">
<inertial>
<origin xyz="0 0.14314 -0.207252" rpy="0 0 0" />
<mass value="0.1" />
<inertia ixx="0.001" ixy = "0" ixz = "0"
iyy="0.001" iyz= "0" izz="0.001" />
</inertial>
<collision name='collision'>
<origin xyz="-0.005 0.14314 -0.207252" rpy="0 1.56893 0" />
<geometry>
<cylinder radius="0.012209" length="0.3" />
</geometry>
</collision>
<collision name='collision_bar'>
<origin xyz="0.00052 0.08503 -0.121187" rpy="0.501318 0 0" />
<geometry>
<cylinder radius="0.00914984" length="0.176893" />
</geometry>
</collision>
<visual name='base_link_right_leg'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/leg1_remeshed_v3.stl" scale="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<joint name='right_leg_joint' type='fixed'>
<child link="right_leg"/>
<parent link="base_link"/>
<!--<origin xyz="0.00026 0.040515 -0.048" rpy="0 0 0" /> -->
<origin xyz="0.00026 0.009 -0.018" rpy="0 0 0" />
<axis xyz="1 0 0"/>
<!--limit velocity="-1" effort="100" lower="0" upper="1" />
<dynamics damping="0.1"/-->
</joint>
<!--LINK 1-->
<link name='rotor_1'>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_1_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_1_visual'>
<origin xyz="-0.211396 -0.119762 -0.0822169" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_ccw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_1_joint' type='revolute'>
<child link="rotor_1"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="0.211396 0.119762 0.0822169" rpy="0 0 0" />
<dynamics damping="0.005"/>
</joint>
<!--LINK 2-->
<link name='rotor_2'>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_2_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_2_visual'>
<origin xyz="0.00187896 -0.242705 -0.0822169" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_cw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_2_joint' type='revolute'>
<child link="rotor_2"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="-0.00187896 0.242705 0.0822169" rpy="0 0 0" /><!--0.242705-->
<dynamics damping="0.005"/>
</joint>
<!--LINK 3-->
<link name='rotor_3'>
<!-- <origin xyz="-0.211396 +0.119762 +0.082219" rpy="0 0 0" /> -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_3_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_3_visual'>
<origin xyz="-0.211396 -0.119762 -0.0822169" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_ccw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_3_joint' type='revolute'>
<child link="rotor_3"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="-0.211396 0.119762 0.0822169 " rpy="0 0 0" />
<dynamics damping="0.005"/>
</joint>
<!--LINK 4-->
<link name='rotor_4'>
<!-- <origin xyz="-0.00187896 -0.242705 0.0822169" rpy="0 0 -2.09439510239" /> -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_4_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_4_visual'>
<origin xyz="-0.211396 -0.119762 -0.082219" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_ccw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_4_joint' type='revolute'>
<child link="rotor_4"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="-0.211396 -0.119762 0.082219" rpy="0 0 0" />
<dynamics damping="0.005"/>
</joint>
<!--LINK 5-->
<link name='rotor_5'>
<!-- <origin xyz="-0.00187896 0.242705 0.0822169" rpy="0 0 0" /> -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_5_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_5_visual'>
<origin xyz="0.00187896 -0.242705 -0.0822169" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_cw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_5_joint' type='revolute'>
<child link="rotor_5"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="0.00187896 -0.242705 0.0822169" rpy="0 0 0" />
<dynamics damping="0.005"/>
</joint>
<!--LINK 6-->
<link name='rotor_6'>
<!-- <origin xyz="-0.209396 0.122762 0.082219" rpy="0 0 2.09439510239" /> -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.005"/>
<inertia ixx="9.75e-07" ixy = "0" ixz = "0"
iyy="0.000273104" iyz= "0" izz="0.000274004" />
</inertial>
<collision name='rotor_6_collision'>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="0.005" radius="0.128"/>
</geometry>
</collision>
<visual name='rotor_6_visual'>
<origin xyz="-0.211396 -0.119762 -0.082219" rpy="0 0 0" />
<geometry>
<mesh filename="package://mobot_urdf/meshes/prop_ccw_assembly_remeshed_v3.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name='rotor_6_joint' type='revolute'>
<child link="rotor_6"/>
<parent link="base_link"/>
<axis xyz="0 0 1"/>
<limit velocity="-1" effort="10" lower="-1e+16" upper="1e+16" />
<origin xyz="0.211396 -0.119762 0.082219" rpy="0 0 0" />
<dynamics damping="0.005"/>
</joint>
<!-- IMU link -->
<link name="typhoon_h480/imu_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.015" /> <!-- [kg] -->
<inertia ixx="1e-05" ixy = "0" ixz = "0"
iyy="1e-05" iyz= "0" izz="1e-05" />
</inertial>
</link>
<!-- IMU joint -->
<joint name="typhoon_h480/imu_joint" type="revolute">
<parent link="base_link" />
<child link="typhoon_h480/imu_link" />
<limit upper="0" lower="0" effort="0" velocity="0" />
</joint>
<gazebo>
<plugin name="zero_link" filename="libgazebo_multirotor_base_plugin.so">
<linkName>base_link</linkName>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/DarkGrey</material>
<collision name='base_link_inertia_collision'>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0.0</max_vel>
</ode>
</contact>
</surface>
</collision>
</gazebo>
<gazebo>
<plugin name="one_motor_model" filename="libgazebo_motor_model.so">
<robotNamespace></robotNamespace>
<jointName>rotor_1_joint</jointName>
<linkName>rotor_1</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>0</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_1">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="two_motor_model" filename="libgazebo_motor_model.so">
<robotNamespace></robotNamespace>
<jointName>rotor_2_joint</jointName>
<linkName>rotor_2</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>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_2">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="three_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>2</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_3">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="four_motor_model" filename="libgazebo_motor_model.so">
<robotNamespace></robotNamespace>
<jointName>rotor_4_joint</jointName>
<linkName>rotor_4</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>3</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_4">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="five_motor_model" filename="libgazebo_motor_model.so">
<robotNamespace></robotNamespace>
<jointName>rotor_5_joint</jointName>
<linkName>rotor_5</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>4</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/4</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_5">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="six_motor_model" filename="libgazebo_motor_model.so">
<robotNamespace></robotNamespace>
<jointName>rotor_6_joint</jointName>
<linkName>rotor_6</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>5</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>0.000001</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/5</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="rotor_6">
<material>Gazebo/Red</material>
</gazebo>
<gazebo>
<plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
<robotNamespace></robotNamespace>
<gpsNoise>true</gpsNoise>
</plugin>
</gazebo>
<gazebo>
<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>
</gazebo>
<gazebo>
<plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
<robotNamespace/>
<pubRate>50</pubRate>
<baroTopic>/baro</baroTopic>
</plugin>
</gazebo>
<gazebo>
<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
<robotNamespace></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>false</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>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="rotor0">
<input_index>0</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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="rotor1">
<input_index>1</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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>2</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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>3</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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>4</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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="rotor5">
<input_index>5</input_index>
<input_offset>0</input_offset>
<input_scaling>1500</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>
</gazebo>
<gazebo>
<plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
<robotNamespace></robotNamespace>
<linkName>typhoon_h480/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>
</gazebo>
</robot>
The model looks normal but can’t fly. Could someone tell me what the problem is?
I thought about redesigning a new hexacopter, but i don’t kown how does the model match the controller.
By the way, is there anyone offer me a correct hexrcopter xacro file? I would be very grateful!!