init
This commit is contained in:
11
submarine_sinking/model.config
Normal file
11
submarine_sinking/model.config
Normal file
@@ -0,0 +1,11 @@
|
||||
<?xml version="1.0" ?>
|
||||
<model>
|
||||
<name>Submarine (sinking)</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.5">model.sdf</sdf>
|
||||
<author>
|
||||
<name>Jackie Kay</name>
|
||||
<email>jackie@osrfoundation.org</email>
|
||||
</author>
|
||||
<description>An underwater submarine model that will sink in water. You can view this model in underwater.world environment: gazebo worlds/underwater.world</description>
|
||||
</model>
|
262
submarine_sinking/model.sdf
Normal file
262
submarine_sinking/model.sdf
Normal file
@@ -0,0 +1,262 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.5'>
|
||||
|
||||
<model name='submarine_sinking'>
|
||||
<pose>0 0 1.5 1.5707963267948966 0 0</pose>
|
||||
<link name='body'>
|
||||
<pose>0 0 -1.07935 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>1102.6990214100174</mass>
|
||||
<inertia>
|
||||
<ixx>645.9978433760353</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>645.9978433760353</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>49.62145596345078</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
|
||||
|
||||
<visual name='body_visual'>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.3</radius>
|
||||
<length>2.6</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name='body_collision'>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.3</radius>
|
||||
<length>2.6</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name='spacer_visual'>
|
||||
<pose>0 0 1.378546 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.0933402</radius>
|
||||
<length>0.127211</length>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='propeller'>
|
||||
<pose>0 0 0.3455 3.14159 0 0</pose>
|
||||
<inertial>
|
||||
<mass>25.2312297</mass>
|
||||
<inertia>
|
||||
<ixx>0.5319073381913637</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.5319073381913637</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>1.0513012375</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
|
||||
<visual name=blade1_visual>
|
||||
<pose>0.3433402 0.0 0 0.7853981633974483 0 0.0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name=blade1_collision>
|
||||
<pose>0.3433402 0.0 0 0.7853981633974483 0 0.0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name=blade2_visual>
|
||||
<pose>0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name=blade2_collision>
|
||||
<pose>0.0 0.3433402 0 0.7853981633974483 0 1.5707963267948966</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name=blade3_visual>
|
||||
<pose>-0.3433402 -0.0 0 -0.7853981633974483 0 0.0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name=blade3_collision>
|
||||
<pose>-0.3433402 -0.0 0 -0.7853981633974483 0 0.0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name=blade4_visual>
|
||||
<pose>-0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name=blade4_collision>
|
||||
<pose>-0.0 -0.3433402 0 -0.7853981633974483 0 1.5707963267948966</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.5 0.154178 0.05455</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name='spinning_joint' type='revolute'>
|
||||
<parent>body</parent>
|
||||
<child>propeller</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
<limit>
|
||||
<lower>-1e+12</lower>
|
||||
<upper>1e+12</upper>
|
||||
<effort>-1</effort>
|
||||
<velocity>-1</velocity>
|
||||
</limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<allow_auto_disable>1</allow_auto_disable>
|
||||
|
||||
|
||||
|
||||
<!-- Commented out because it behaves poorly -->
|
||||
<!--
|
||||
<plugin name="submarine_propeller_1" filename="libLiftDragPlugin.so">
|
||||
<air_density>999.1026</air_density>
|
||||
<cla>1.2535816618911175</cla>
|
||||
<cla_stall>-1.4326647564469914</cla_stall>
|
||||
<cda>0</cda>
|
||||
<cda_stall>1.4326647564469914</cda_stall>
|
||||
<alpha_stall>1.396</alpha_stall>
|
||||
<a0>0</a0>
|
||||
|
||||
<area>0.22554881980000002</area>
|
||||
<upward>0 -0.7071067811865476 -0.7071067811865475</upward>
|
||||
<forward>0 -0.7071067811865475 0.7071067811865476</forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>0.35 0 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_2" filename="libLiftDragPlugin.so">
|
||||
<air_density>999.1026</air_density>
|
||||
<cla>1.2535816618911175</cla>
|
||||
<cla_stall>-1.4326647564469914</cla_stall>
|
||||
<cda>0</cda>
|
||||
<cda_stall>1.4326647564469914</cda_stall>
|
||||
<alpha_stall>1.396</alpha_stall>
|
||||
<a0>0</a0>
|
||||
|
||||
<area>0.22554881980000002</area>
|
||||
<upward>-0.7071067811865475 0 -0.7071067811865476</upward>
|
||||
<forward>-0.7071067811865476 0 0.7071067811865475</forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>0 -0.35 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_3" filename="libLiftDragPlugin.so">
|
||||
<air_density>999.1026</air_density>
|
||||
<cla>1.2535816618911175</cla>
|
||||
<cla_stall>-1.4326647564469914</cla_stall>
|
||||
<cda>0</cda>
|
||||
<cda_stall>1.4326647564469914</cda_stall>
|
||||
<alpha_stall>1.396</alpha_stall>
|
||||
<a0>0</a0>
|
||||
|
||||
<area>0.22554881980000002</area>
|
||||
<upward>0 0.7071067811865475 -0.7071067811865476</upward>
|
||||
<forward>0 -0.7071067811865476 -0.7071067811865475</forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>-0.35 0 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_4" filename="libLiftDragPlugin.so">
|
||||
<air_density>999.1026</air_density>
|
||||
<cla>1.2535816618911175</cla>
|
||||
<cla_stall>-1.4326647564469914</cla_stall>
|
||||
<cda>0</cda>
|
||||
<cda_stall>1.4326647564469914</cda_stall>
|
||||
<alpha_stall>1.396</alpha_stall>
|
||||
<a0>0</a0>
|
||||
|
||||
<area>0.22554881980000002</area>
|
||||
<upward>0.7071067811865476 0 -0.7071067811865475</upward>
|
||||
<forward>0.7071067811865475 0 0.7071067811865476</forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>0 0.35 0</cp>
|
||||
</plugin>
|
||||
-->
|
||||
|
||||
<plugin name="buoyancy" filename="libBuoyancyPlugin.so">
|
||||
<fluid_density>999.1026</fluid_density>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
197
submarine_sinking/model.sdf.erb
Normal file
197
submarine_sinking/model.sdf.erb
Normal file
@@ -0,0 +1,197 @@
|
||||
<?xml version='1.0'?>
|
||||
<sdf version='1.5'>
|
||||
<%
|
||||
body_density = 1500
|
||||
body_radius = 0.3
|
||||
body_length = 2.6
|
||||
body_volume = body_length*Math::PI*body_radius**2
|
||||
spacer_radius = 0.0933402
|
||||
spacer_length = 0.127211
|
||||
|
||||
mass = body_density*(body_volume)
|
||||
|
||||
body_ixx = mass/12.0 * (3*body_radius**2 + body_length**2 )
|
||||
body_iyy = mass/12.0 * (3*body_radius**2 + body_length**2 )
|
||||
body_izz = mass/2.0 * body_radius**2
|
||||
|
||||
blade_dx = 0.5
|
||||
blade_dy = 0.154178
|
||||
blade_dz = 0.05455
|
||||
blade_volume = blade_dx * blade_dy * blade_dz
|
||||
blade_mass = blade_volume * body_density
|
||||
|
||||
total_blade_mass = 4*blade_mass
|
||||
|
||||
blade_ixx = total_blade_mass/12.0 * (blade_dx**2 + blade_dz**2)
|
||||
blade_iyy = total_blade_mass/12.0 * (blade_dz**2 + blade_dx**2)
|
||||
blade_izz = total_blade_mass/12.0 * (blade_dx**2 + blade_dx**2)
|
||||
|
||||
blade_angle = 45 * Math::PI/180
|
||||
|
||||
fluid_density = 999.1026
|
||||
|
||||
material_block =
|
||||
'<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Black</name>
|
||||
</script>
|
||||
</material>'
|
||||
%>
|
||||
<model name='submarine_sinking'>
|
||||
<pose>0 0 <%= blade_dx + 1.0 %> <%= Math::PI/2%> 0 0</pose>
|
||||
<link name='body'>
|
||||
<pose>0 0 -1.07935 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass><%= mass %></mass>
|
||||
<inertia>
|
||||
<ixx><%= body_ixx %></ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy><%= body_iyy %></iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz><%= body_izz %></izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<% body_geometry =
|
||||
'<geometry>
|
||||
<cylinder>
|
||||
<radius>'+ body_radius.to_s + '</radius>
|
||||
<length>'+ body_length.to_s + '</length>
|
||||
</cylinder>
|
||||
</geometry>'
|
||||
%>
|
||||
|
||||
<visual name='body_visual'>
|
||||
<%= body_geometry%>
|
||||
<%= material_block%>
|
||||
</visual>
|
||||
<collision name='body_collision'>
|
||||
<%= body_geometry%>
|
||||
</collision>
|
||||
<% spacer_geometry_pose =
|
||||
'<pose>0 0 ' + (0.299196 + 1.07935).to_s + ' 0 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>'+ spacer_radius.to_s + '</radius>
|
||||
<length>'+ spacer_length.to_s + '</length>
|
||||
</cylinder>
|
||||
</geometry>'
|
||||
%>
|
||||
<visual name='spacer_visual'>
|
||||
<%= spacer_geometry_pose%>
|
||||
<%= material_block%>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<link name='propeller'>
|
||||
<pose>0 0 0.3455 3.14159 0 0</pose>
|
||||
<inertial>
|
||||
<mass><%= total_blade_mass %></mass>
|
||||
<inertia>
|
||||
<ixx><%= blade_ixx %></ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy><%= blade_iyy %></iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz><%= blade_izz %></izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<% for i in 1..4
|
||||
x = (i%2)*(spacer_radius + blade_dx/2)*(if i > 2 then -1 else 1 end)
|
||||
y = ((i+1)%2)*(spacer_radius + blade_dx/2)*(if i > 2 then -1 else 1 end)
|
||||
roll = blade_angle*(if i > 2 then -1 else 1 end)
|
||||
yaw = ((i+1)%2)*Math::PI/2
|
||||
|
||||
geometry_pose =
|
||||
'<pose>' + x.to_s + ' ' + y.to_s + ' 0 ' + roll.to_s + ' 0 ' + yaw.to_s + '</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>' + blade_dx.to_s + ' ' + blade_dy.to_s + ' ' + blade_dz.to_s + '</size>
|
||||
</box>
|
||||
</geometry>'
|
||||
%>
|
||||
<visual name=<%= 'blade' + i.to_s + '_visual'%>>
|
||||
<%= geometry_pose %>
|
||||
<%= material_block%>
|
||||
</visual>
|
||||
<collision name=<%= 'blade' + i.to_s + '_collision'%>>
|
||||
<%= geometry_pose %>
|
||||
</collision>
|
||||
<% end %>
|
||||
</link>
|
||||
|
||||
<joint name='spinning_joint' type='revolute'>
|
||||
<parent>body</parent>
|
||||
<child>propeller</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
<limit>
|
||||
<lower>-1e+12</lower>
|
||||
<upper>1e+12</upper>
|
||||
<effort>-1</effort>
|
||||
<velocity>-1</velocity>
|
||||
</limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<allow_auto_disable>1</allow_auto_disable>
|
||||
|
||||
<% lift_drag_properties =
|
||||
"<air_density>" + fluid_density.to_s + "</air_density>\n" +
|
||||
" <cla>" + (1.75/1.396).to_s + "</cla>\n" +
|
||||
" <cla_stall>" + (-0.25/0.1745).to_s + "</cla_stall>\n" +
|
||||
" <cda>0</cda>\n" +
|
||||
" <cda_stall>" + (2/1.396).to_s + "</cda_stall>\n" +
|
||||
" <alpha_stall>" + 1.396.to_s + "</alpha_stall>\n" +
|
||||
" <a0>0</a0>\n"
|
||||
%>
|
||||
|
||||
<!-- Commented out because it behaves poorly -->
|
||||
<!--
|
||||
<plugin name="submarine_propeller_1" filename="libLiftDragPlugin.so">
|
||||
<%= lift_drag_properties%>
|
||||
<area><%= blade_dx*blade_dy*2 + blade_dx*blade_dz*2 + blade_dy*blade_dz*2 %></area>
|
||||
<upward>0 <%= -Math.cos(blade_angle) %> <%= -Math.sin(blade_angle) %></upward>
|
||||
<forward>0 <%= -Math.sin(blade_angle) %> <%= Math.cos(blade_angle) %></forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp><%= blade_dx*0.7 %> 0 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_2" filename="libLiftDragPlugin.so">
|
||||
<%= lift_drag_properties%>
|
||||
<area><%= blade_dx*blade_dy*2 + blade_dx*blade_dz*2 + blade_dy*blade_dz*2 %></area>
|
||||
<upward><%= -Math.sin(blade_angle) %> 0 <%= -Math.cos(blade_angle) %></upward>
|
||||
<forward><%= -Math.cos(blade_angle) %> 0 <%= Math.sin(blade_angle) %></forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>0 <%= -blade_dx*0.7 %> 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_3" filename="libLiftDragPlugin.so">
|
||||
<%= lift_drag_properties%>
|
||||
<area><%= blade_dx*blade_dy*2 + blade_dx*blade_dz*2 + blade_dy*blade_dz*2 %></area>
|
||||
<upward>0 <%= Math.sin(blade_angle) %> <%= -Math.cos(blade_angle) %></upward>
|
||||
<forward>0 <%= -Math.cos(blade_angle) %> <%= -Math.sin(blade_angle) %></forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp><%= -blade_dx*0.7 %> 0 0</cp>
|
||||
</plugin>
|
||||
|
||||
<plugin name="submarine_propeller_4" filename="libLiftDragPlugin.so">
|
||||
<%= lift_drag_properties%>
|
||||
<area><%= blade_dx*blade_dy*2 + blade_dx*blade_dz*2 + blade_dy*blade_dz*2 %></area>
|
||||
<upward><%= Math.cos(blade_angle) %> 0 <%= -Math.sin(blade_angle) %></upward>
|
||||
<forward><%= Math.sin(blade_angle) %> 0 <%= Math.cos(blade_angle) %></forward>
|
||||
<link_name>propeller</link_name>
|
||||
<cp>0 <%= blade_dx*0.7 %> 0</cp>
|
||||
</plugin>
|
||||
-->
|
||||
|
||||
<plugin name="buoyancy" filename="libBuoyancyPlugin.so">
|
||||
<fluid_density><%= fluid_density%></fluid_density>
|
||||
</plugin>
|
||||
|
||||
</model>
|
||||
</sdf>
|
Reference in New Issue
Block a user