ppovb5fc7/gazebo/test/worlds/friction_spheres.world.erb

<?xml version="1.0" ?>
<%= "<!-- this file was generated using embedded ruby -->" %>
<sdf version='1.6'>
  <world name='default'>
    <gui>
      <camera name='user_camera'>
        <pose>17 3 22 0 1.1 <%= Math::PI %></pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <gravity>0 2 -9.81</gravity>
<%
  # World with several models with varying number of collision spheres
  # SI units (length in meters)
  require "matrix"
  def a_to_s(v)
    Array(v).join(" ")
  end

  # Geometry
  ball_radius   = 0.2
  face_altitude = 0.5
  side_length   = face_altitude / (Math::sqrt(3) / 2.0)
  ball_volume   = 4.0 / 3.0 * Math::PI * ball_radius**3
  def sphere_collision(name_suffix, pos, radius, color="Green", slip=1, friction=0.5)
    """<collision name='collision_#{name_suffix}'>
          <pose>#{ a_to_s(pos) }  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>#{ radius }</radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>#{ friction }</mu>
                <mu2>#{ friction }</mu2>
                <slip1>#{ slip }</slip1>
                <slip2>#{ slip }</slip2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual_#{ name_suffix }'>
          <pose>#{ a_to_s(pos) }  0 0 0</pose>
          <geometry>
            <sphere>
              <radius>#{ radius }</radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/#{ color }</name>
            </script>
          </material>
        </visual>"""
  end

  models = {}
  # "lowball" has one collision sphere with a hanging center of mass,
  # which makes the ball slide instead of roll
  models["lowball"] = {
    :inertia_offset => Vector[0, 0, -0.3],
    :ball_locations => {
      "a" => Vector[0, 0, 0] ,
    }
  }
  # "twoball" has two collision spheres
  # it will slide in one direction and roll in the other direction
  models["twoball"] = {
    :ball_locations => {
      "a" => Vector[0,  side_length / 2.0, 0] ,
      "b" => Vector[0, -side_length / 2.0, 0] ,
    }
  }
  # "triball" has three collision spheres arranged in an equilateral triangle
  # it will slide in any direction
  models["triball"] = {
    :ball_locations => {
      "a" => Vector[ 2.0 / 3.0 * face_altitude, 0,                  0] ,
      "b" => Vector[-1.0 / 3.0 * face_altitude,  side_length / 2.0, 0] ,
      "c" => Vector[-1.0 / 3.0 * face_altitude, -side_length / 2.0, 0] ,
    }
  }

  # there are several pairs of test cases
  # each pair has the same product of mass and slip,
  mass_slip = {}
  mass_slip["0.2a"] = { :mass => 1.0, :slip => 0.2, :color => "Red"}
  mass_slip["0.2b"] = { :mass => 2.0, :slip => 0.1, :color => "Red"}
  mass_slip["0.4a"] = { :mass => 2.0, :slip => 0.2, :color => "Green"}
  mass_slip["0.4b"] = { :mass => 4.0, :slip => 0.1, :color => "Green"}
  mass_slip["0.6a"] = { :mass => 3.0, :slip => 0.2, :color => "Blue"}
  mass_slip["0.6b"] = { :mass => 6.0, :slip => 0.1, :color => "Blue"}

  models.keys.each_with_index do |model_prefix, i|
    model = models[model_prefix]
    ball_locations = model[:ball_locations]
    ball_count = ball_locations.length
    inertia_offset = model[:inertia_offset]
    inertia_offset = model.key?(:inertia_offset) ? model[:inertia_offset] : Vector[0, 0, 0]

    mass_slip.keys.each_with_index do |ms_name, ii|
      ms = mass_slip[ms_name]
      mass = ms[:mass]
      slip = ms[:slip]
      color = ms[:color]

      # inertia
      link_mass = mass
      ball_mass = link_mass * 1.0/ball_count
      ball_ixx  = 2.0/5.0 * ball_mass * ball_radius**2
      ball_iyy  = 2.0/5.0 * ball_mass * ball_radius**2
      ball_izz  = 2.0/5.0 * ball_mass * ball_radius**2

      # center of mass (com) and lumped inertia of balls
      ball_com = Vector[0, 0, 0]
      ball_locations.keys.each do |k|
        ball_com += ball_locations[k] / ball_count
      end
      link_ixx = ball_count * ball_ixx
      link_iyy = ball_count * ball_iyy
      link_izz = ball_count * ball_izz
      link_ixy = 0.0
      link_ixz = 0.0
      link_iyz = 0.0
      ball_locations.keys.each do |k|
        dx = ball_locations[k][0] - ball_com[0]
        dy = ball_locations[k][1] - ball_com[1]
        dz = ball_locations[k][2] - ball_com[2]
        link_ixx += ball_mass * (dy**2 + dz**2)
        link_iyy += ball_mass * (dz**2 + dx**2)
        link_izz += ball_mass * (dx**2 + dy**2)
        link_ixy -= ball_mass * dx*dy
        link_ixz -= ball_mass * dx*dz
        link_iyz -= ball_mass * dy*dz
      end

      model_name = "#{model_prefix}_#{ms_name}"
      model_pose = Vector[2*ii, -2*i, 0, 0, 0, 0]
%>
    <model name="<%= model_name %>">
      <pose><%= a_to_s(model_pose) %></pose>
      <link name="link">
        <pose>0 0 <%= ball_radius %>  0 0 0</pose>
        <inertial>
          <pose><%= a_to_s(ball_com + inertia_offset) %>  0 0 0</pose>
          <mass><%= link_mass %></mass>
          <inertia>
            <ixx><%= link_ixx %></ixx>
            <iyy><%= link_iyy %></iyy>
            <izz><%= link_izz %></izz>
            <ixy><%= link_ixy %></ixy>
            <ixz><%= link_ixz %></ixz>
            <iyz><%= link_iyz %></iyz>
          </inertia>
        </inertial>
<%
      # Place sphere collision and visual at each corner
      ball_locations.keys.each_index do |first|
        key = ball_locations.keys[first]
        pos = ball_locations[key]
%>
        <%= sphere_collision(key, pos, ball_radius, color, slip) %>
<%
      end
%>
      </link>
    </model>
<%
    end
  end
%>
  </world>
</sdf>