328 lines
8.7 KiB
Plaintext
328 lines
8.7 KiB
Plaintext
<?xml version="1.0" ?>
|
|
<%= "<!-- this file was generated using embedded ruby -->" %>
|
|
<%
|
|
# Torsional friction test world
|
|
# Spheres with single points of contact. Torque about the vertical axis
|
|
# will be applied and the torsional friction should resist motion.
|
|
# There are 4 rows of spheres:
|
|
# 1st: different masses, resulting in different contact depths.
|
|
# 2nd: different coefficient
|
|
# 3rd: different patch_radius
|
|
# 4th: different surface radius
|
|
|
|
# Varying parameters
|
|
densities = [100.0, 1000.0, 2700.0, 5000.0, 10000.0]
|
|
coefficients = [0.0, 0.001, 0.005, 0.01, 1.0]
|
|
patch_radii = [0.0, 0.1, 2.0, 5.0, 10.0]
|
|
radii = [0.01, 0.1, 0.8, 2.0, 10.0]
|
|
|
|
# Default parameters
|
|
# Aluminum 2700 kg/m^3
|
|
default_density = 2700.0
|
|
default_coefficient = 0.005
|
|
default_radius = 0.1
|
|
default_patch_radius = 2.0
|
|
|
|
# Kp to make spheres soft
|
|
kp = 500000
|
|
%>
|
|
<sdf version="1.5">
|
|
<world name="default">
|
|
<physics type="ode">
|
|
<ode>
|
|
<solver>
|
|
<type>quick</type>
|
|
<iters>1000</iters>
|
|
<precon_iters>0</precon_iters>
|
|
<sor>1.0</sor>
|
|
</solver>
|
|
<constraints>
|
|
<cfm>0.000000</cfm>
|
|
<erp>0.200000</erp>
|
|
<contact_max_correcting_vel>1000.0</contact_max_correcting_vel>
|
|
<contact_surface_layer>0.0</contact_surface_layer>
|
|
</constraints>
|
|
</ode>
|
|
</physics>
|
|
<gui>
|
|
<camera name="user_camera">
|
|
<pose>-4 1.5 1.9 0 0.17 -0.014</pose>
|
|
</camera>
|
|
</gui>
|
|
<include>
|
|
<uri>model://ground_plane</uri>
|
|
</include>
|
|
<include>
|
|
<uri>model://sun</uri>
|
|
</include>
|
|
|
|
<% # Different masses / contact depth
|
|
radius = default_radius
|
|
coefficient = default_coefficient
|
|
|
|
i = 0
|
|
densities.each do |density|
|
|
i = i + 1
|
|
name = 'sphere_mass_' + i.to_s
|
|
mass = density * 4.0 * Math::PI / 3.0 * radius **3
|
|
ixx = mass * 0.4 * radius**2
|
|
iyy = ixx
|
|
izz = ixx
|
|
%>
|
|
<%= "<model name='#{name}'>" %>
|
|
<allow_auto_disable>0</allow_auto_disable>
|
|
<pose><%= radius*4*i %> 0 <%= radius %> 0 0 0</pose>
|
|
<link name="link">
|
|
<inertial>
|
|
<mass><%= mass %></mass>
|
|
<inertia>
|
|
<ixx><%= ixx %></ixx>
|
|
<iyy><%= iyy %></iyy>
|
|
<izz><%= izz %></izz>
|
|
<ixy>0.0</ixy>
|
|
<ixz>0.0</ixz>
|
|
<iyz>0.0</iyz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<surface>
|
|
<friction>
|
|
<torsional>
|
|
<coefficient><%= coefficient %></coefficient>
|
|
</torsional>
|
|
</friction>
|
|
<contact>
|
|
<ode>
|
|
<kp><%= kp %></kp>
|
|
<max_vel>10000</max_vel>
|
|
</ode>
|
|
</contact>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<material>
|
|
<script>
|
|
<uri>file://media/materials/scripts/gazebo.material</uri>
|
|
<name>Gazebo/CoM</name>
|
|
</script>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
<% end %>
|
|
|
|
<% # Different torsional frictions
|
|
radius = default_radius
|
|
density = default_density
|
|
patch_radius = default_patch_radius
|
|
|
|
mass = density * 4.0 * Math::PI / 3.0 * radius **3
|
|
ixx = mass * 0.4 * radius**2
|
|
iyy = ixx
|
|
izz = ixx
|
|
|
|
i = 0
|
|
coefficients.each do |coefficient|
|
|
i = i + 1
|
|
name = 'sphere_coefficient_' + i.to_s
|
|
%>
|
|
<%= "<model name='#{name}'>" %>
|
|
<allow_auto_disable>0</allow_auto_disable>
|
|
<pose><%= radius*4*i %> 1 <%= radius %> 0 0 0</pose>
|
|
<link name="link">
|
|
<inertial>
|
|
<mass><%= mass %></mass>
|
|
<inertia>
|
|
<ixx><%= ixx %></ixx>
|
|
<iyy><%= iyy %></iyy>
|
|
<izz><%= izz %></izz>
|
|
<ixy>0.0</ixy>
|
|
<ixz>0.0</ixz>
|
|
<iyz>0.0</iyz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<surface>
|
|
<friction>
|
|
<torsional>
|
|
<coefficient><%= coefficient %></coefficient>
|
|
<patch_radius><%= patch_radius %></patch_radius>
|
|
</torsional>
|
|
</friction>
|
|
<contact>
|
|
<ode>
|
|
<kp><%= kp %></kp>
|
|
</ode>
|
|
</contact>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<material>
|
|
<script>
|
|
<uri>file://media/materials/scripts/gazebo.material</uri>
|
|
<name>Gazebo/CoM</name>
|
|
</script>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
<% end %>
|
|
|
|
<% # Different patch radii
|
|
radius = default_radius
|
|
density = default_density
|
|
coefficient = default_coefficient
|
|
|
|
mass = density * 4.0 * Math::PI / 3.0 * radius **3
|
|
ixx = mass * 0.4 * radius**2
|
|
iyy = ixx
|
|
izz = ixx
|
|
|
|
i = 0
|
|
patch_radii.each do |patch_radius|
|
|
i = i + 1
|
|
name = 'sphere_patch_' + i.to_s
|
|
%>
|
|
<%= "<model name='#{name}'>" %>
|
|
<allow_auto_disable>0</allow_auto_disable>
|
|
<pose><%= radius*4*i %> 2 <%= radius %> 0 0 0</pose>
|
|
<link name="link">
|
|
<inertial>
|
|
<mass><%= mass %></mass>
|
|
<inertia>
|
|
<ixx><%= ixx %></ixx>
|
|
<iyy><%= iyy %></iyy>
|
|
<izz><%= izz %></izz>
|
|
<ixy>0.0</ixy>
|
|
<ixz>0.0</ixz>
|
|
<iyz>0.0</iyz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<surface>
|
|
<friction>
|
|
<torsional>
|
|
<coefficient><%= coefficient %></coefficient>
|
|
<patch_radius><%= patch_radius %></patch_radius>
|
|
<use_patch_radius>true</use_patch_radius>
|
|
</torsional>
|
|
</friction>
|
|
<contact>
|
|
<ode>
|
|
<kp><%= kp %></kp>
|
|
</ode>
|
|
</contact>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<material>
|
|
<script>
|
|
<uri>file://media/materials/scripts/gazebo.material</uri>
|
|
<name>Gazebo/CoM</name>
|
|
</script>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
<% end %>
|
|
|
|
<% # Different surface radius
|
|
density = default_density
|
|
coefficient = default_coefficient
|
|
|
|
mass = density * 4.0 * Math::PI / 3.0 * radius **3
|
|
ixx = mass * 0.4 * radius**2
|
|
iyy = ixx
|
|
izz = ixx
|
|
|
|
i = 0
|
|
radii.each do |radius|
|
|
i = i + 1
|
|
name = 'sphere_radius_' + i.to_s
|
|
%>
|
|
<%= "<model name='#{name}'>" %>
|
|
<allow_auto_disable>0</allow_auto_disable>
|
|
<pose><%= radius*4*i %> 3 <%= radius %> 0 0 0</pose>
|
|
<link name="link">
|
|
<inertial>
|
|
<mass><%= mass %></mass>
|
|
<inertia>
|
|
<ixx><%= ixx %></ixx>
|
|
<iyy><%= iyy %></iyy>
|
|
<izz><%= izz %></izz>
|
|
<ixy>0.0</ixy>
|
|
<ixz>0.0</ixz>
|
|
<iyz>0.0</iyz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<surface>
|
|
<friction>
|
|
<torsional>
|
|
<coefficient><%= coefficient %></coefficient>
|
|
<surface_radius><%= radius %></surface_radius>
|
|
<use_patch_radius>false</use_patch_radius>
|
|
</torsional>
|
|
</friction>
|
|
<contact>
|
|
<ode>
|
|
<kp><%= kp %></kp>
|
|
</ode>
|
|
</contact>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<sphere>
|
|
<radius><%= radius %></radius>
|
|
</sphere>
|
|
</geometry>
|
|
<material>
|
|
<script>
|
|
<uri>file://media/materials/scripts/gazebo.material</uri>
|
|
<name>Gazebo/CoM</name>
|
|
</script>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
<% end %>
|
|
|
|
</world>
|
|
</sdf>
|