<?xml version="1.0" ?>
<%
  # 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>
