init
This commit is contained in:
570
iris_with_standoffs_demo/model.sdf
Normal file
570
iris_with_standoffs_demo/model.sdf
Normal file
@@ -0,0 +1,570 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version="1.6">
|
||||
<model name="iris_demo">
|
||||
<include>
|
||||
<uri>model://iris_with_standoffs</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<uri>model://gimbal_small_2d</uri>
|
||||
<pose>0 -0.01 0.070 1.57 0 1.57</pose>
|
||||
</include>
|
||||
|
||||
<joint name="iris_gimbal_mount" type="revolute">
|
||||
<parent>iris::base_link</parent>
|
||||
<child>gimbal_small_2d::base_link</child>
|
||||
<axis>
|
||||
<limit>
|
||||
<lower>0</lower>
|
||||
<upper>0</upper>
|
||||
</limit>
|
||||
<xyz>0 0 1</xyz>
|
||||
<use_parent_model_frame>true</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- visual markers for debugging
|
||||
<link name="rotor_0_blade_1_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>0.13 -0.22 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_0_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_tip'>
|
||||
<pose>0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp'>
|
||||
<pose>0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp_forward'>
|
||||
<pose>0.084 0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp_upward'>
|
||||
<pose>0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="rotor_0_blade_2_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>0.13 -0.22 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_0_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_tip'>
|
||||
<pose>-0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp'>
|
||||
<pose>-0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp_forward'>
|
||||
<pose>-0.084 -0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_0_visual_cp_upward'>
|
||||
<pose>-0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="rotor_1_blade_1_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>-0.13 0.2 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_1_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_tip'>
|
||||
<pose>0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp'>
|
||||
<pose>0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp_forward'>
|
||||
<pose>0.084 0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp_upward'>
|
||||
<pose>0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="rotor_1_blade_2_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>-0.13 0.2 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_1_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_tip'>
|
||||
<pose>-0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp'>
|
||||
<pose>-0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp_forward'>
|
||||
<pose>-0.084 -0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_1_visual_cp_upward'>
|
||||
<pose>-0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="rotor_2_blade_1_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>0.13 0.22 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_2_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_tip'>
|
||||
<pose>0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp'>
|
||||
<pose>0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp_forward'>
|
||||
<pose>0.084 -0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp_upward'>
|
||||
<pose>0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="rotor_2_blade_2_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>0.13 0.22 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_2_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_tip'>
|
||||
<pose>-0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp'>
|
||||
<pose>-0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp_forward'>
|
||||
<pose>-0.084 0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_2_visual_cp_upward'>
|
||||
<pose>-0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name="rotor_3_blade_1_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>-0.13 -0.2 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_3_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_tip'>
|
||||
<pose>0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp'>
|
||||
<pose>0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp_forward'>
|
||||
<pose>0.084 -0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp_upward'>
|
||||
<pose>0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="rotor_3_blade_2_cp">
|
||||
<gravity>0</gravity>
|
||||
<pose>-0.13 -0.2 0.216 0 -0 0</pose>
|
||||
<visual name='rotor_3_visual_root'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_tip'>
|
||||
<pose>-0.12 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.01</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp'>
|
||||
<pose>-0.084 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp_forward'>
|
||||
<pose>-0.084 0.02 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='rotor_3_visual_cp_upward'>
|
||||
<pose>-0.084 0 0.02 0 -0 0</pose>
|
||||
<geometry>
|
||||
<sphere>
|
||||
<radius>0.003</radius>
|
||||
</sphere>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
-->
|
||||
|
||||
<!-- plugins -->
|
||||
<plugin name="rotor_0_blade_1" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>0.084 0 0</cp>
|
||||
<forward>0 1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_0</link_name>
|
||||
</plugin>
|
||||
<plugin name="rotor_0_blade_2" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>-0.084 0 0</cp>
|
||||
<forward>0 -1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_0</link_name>
|
||||
</plugin>
|
||||
|
||||
<plugin name="rotor_1_blade_1" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>0.084 0 0</cp>
|
||||
<forward>0 1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_1</link_name>
|
||||
</plugin>
|
||||
<plugin name="rotor_1_blade_2" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>-0.084 0 0</cp>
|
||||
<forward>0 -1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_1</link_name>
|
||||
</plugin>
|
||||
|
||||
<plugin name="rotor_2_blade_1" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>0.084 0 0</cp>
|
||||
<forward>0 -1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_2</link_name>
|
||||
</plugin>
|
||||
<plugin name="rotor_2_blade_2" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>-0.084 0 0</cp>
|
||||
<forward>0 1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_2</link_name>
|
||||
</plugin>
|
||||
|
||||
<plugin name="rotor_3_blade_1" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>0.084 0 0</cp>
|
||||
<forward>0 -1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_3</link_name>
|
||||
</plugin>
|
||||
<plugin name="rotor_3_blade_2" filename="libLiftDragPlugin.so">
|
||||
<a0>0.1</a0>
|
||||
<alpha_stall>1.4</alpha_stall>
|
||||
<cla>0.2500</cla>
|
||||
<cda>0.10</cda>
|
||||
<cma>0.00</cma>
|
||||
<cla_stall>-0.025</cla_stall>
|
||||
<cda_stall>0.0</cda_stall>
|
||||
<cma_stall>0.0</cma_stall>
|
||||
<area>0.2</area>
|
||||
<air_density>1.2041</air_density>
|
||||
<cp>-0.084 0 0</cp>
|
||||
<forward>0 1 0</forward>
|
||||
<upward>0 0 1</upward>
|
||||
<link_name>iris::rotor_3</link_name>
|
||||
</plugin>
|
||||
<plugin name="arducopter_plugin" filename="libArduCopterPlugin.so">
|
||||
<imuName>iris_demo::iris::iris/imu_link::imu_sensor</imuName>
|
||||
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
|
||||
<rotor id="0">
|
||||
<vel_p_gain>0.01</vel_p_gain>
|
||||
<vel_i_gain>0</vel_i_gain>
|
||||
<vel_d_gain>0</vel_d_gain>
|
||||
<vel_i_max>0</vel_i_max>
|
||||
<vel_i_min>0</vel_i_min>
|
||||
<vel_cmd_max>2.0</vel_cmd_max>
|
||||
<vel_cmd_min>-2.0</vel_cmd_min>
|
||||
<jointName>iris::rotor_0_joint</jointName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<rotorVelocitySlowdownSim>1</rotorVelocitySlowdownSim>
|
||||
</rotor>
|
||||
<rotor id="1">
|
||||
<vel_p_gain>0.01</vel_p_gain>
|
||||
<vel_i_gain>0</vel_i_gain>
|
||||
<vel_d_gain>0</vel_d_gain>
|
||||
<vel_i_max>0</vel_i_max>
|
||||
<vel_i_min>0</vel_i_min>
|
||||
<vel_cmd_max>2.0</vel_cmd_max>
|
||||
<vel_cmd_min>-2.0</vel_cmd_min>
|
||||
<jointName>iris::rotor_1_joint</jointName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<rotorVelocitySlowdownSim>1</rotorVelocitySlowdownSim>
|
||||
</rotor>
|
||||
<rotor id="2">
|
||||
<vel_p_gain>0.01</vel_p_gain>
|
||||
<vel_i_gain>0</vel_i_gain>
|
||||
<vel_d_gain>0</vel_d_gain>
|
||||
<vel_i_max>0</vel_i_max>
|
||||
<vel_i_min>0</vel_i_min>
|
||||
<vel_cmd_max>2.0</vel_cmd_max>
|
||||
<vel_cmd_min>-2.0</vel_cmd_min>
|
||||
<jointName>iris::rotor_2_joint</jointName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<rotorVelocitySlowdownSim>1</rotorVelocitySlowdownSim>
|
||||
</rotor>
|
||||
<rotor id="3">
|
||||
<vel_p_gain>0.01</vel_p_gain>
|
||||
<vel_i_gain>0</vel_i_gain>
|
||||
<vel_d_gain>0</vel_d_gain>
|
||||
<vel_i_max>0</vel_i_max>
|
||||
<vel_i_min>0</vel_i_min>
|
||||
<vel_cmd_max>2.0</vel_cmd_max>
|
||||
<vel_cmd_min>-2.0</vel_cmd_min>
|
||||
<jointName>iris::rotor_3_joint</jointName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<rotorVelocitySlowdownSim>1</rotorVelocitySlowdownSim>
|
||||
</rotor>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
Reference in New Issue
Block a user