<?xml version="1.0" ?>
<%= "<!-- this file was generated using embedded ruby -->" %>
<sdf version='1.6'>
  <world name='default'>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <gravity>0 1 -9.81</gravity>
    <physics type="ode">
      <dart>
        <collision_detector>bullet</collision_detector>
      </dart>
    </physics>
<%
  # Test pyramid friction model
  # Set asymmetric friction coefficients and friction direction
  # Then apply lateral gravity component and watch the angle that boxes slide
  # Also add some sphere and spin them to make sure friction directions
  # are in the body-fixed frame
  require 'matrix'
  def a_to_s(v)
    Array(v).join(" ")
  end

  sphere = {}
  sphere[:radius] = 0.25
  sphere[:mass] = 1
  sphere[:ixx] = 0.4 * sphere[:mass] * sphere[:radius]**2
  sphere[:iyy] = sphere[:ixx]
  sphere[:izz] = sphere[:ixx]
  sphere[:y0] = 3
%>
    <model name='spheres'>
      <pose>0 <%= sphere[:y0] %> <%= sphere[:radius] %>  0 0 0</pose>
      <link name='link_1'>
        <inertial>
          <mass><%= sphere[:mass] %></mass>
          <inertia>
            <ixx><%= sphere[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= sphere[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= sphere[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius><%= sphere[:radius] %></radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1>1 0 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius><%= sphere[:radius] %></radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
      <link name='link_2'>
        <pose>0 1 0  0 0 0</pose>
        <inertial>
          <mass><%= sphere[:mass] %></mass>
          <inertia>
            <ixx><%= sphere[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= sphere[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= sphere[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <sphere>
              <radius><%= sphere[:radius] %></radius>
            </sphere>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1>1 0 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <sphere>
              <radius><%= sphere[:radius] %></radius>
            </sphere>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
<%
  # box dimensions
  dx = 0.5
  dy = dx
  dz = 0.2

  box = {}
  box[:size] = Vector[dx, dy, dz]
  box[:mass] = 1
  box[:ixx] = box[:mass] / 12.0 * (dy**2 + dz**2)
  box[:iyy] = box[:mass] / 12.0 * (dz**2 + dx**2)
  box[:izz] = box[:mass] / 12.0 * (dx**2 + dy**2)

  box_count = 10
  box_angles = Math::PI * Vector[*(0..box_count)] / box_count

  rings = {}
  rings["fdir"] = {:radius => 5}
  rings["collision"] = {:radius => 6}
  rings["link"] = {:radius => 7}
  rings["model"] = {:radius => 8}

  ring_name = "fdir"
  ring = rings[ring_name]
  box_angles.each_with_index do |a,i|
    name = "box_#{ring_name}_#{i}"
    pos_xy = ring[:radius] * Vector[Math.cos(a), Math.sin(a)]
    fdir = Vector[-Math.sin(a), Math.cos(a)]
%>
    <model name='<%= name %>'>
      <pose><%= a_to_s(pos_xy) %> 0.1 0 -0 0</pose>
      <link name='link_1'>
        <inertial>
          <mass><%= box[:mass] %></mass>
          <inertia>
            <ixx><%= box[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= box[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= box[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1><%= a_to_s(fdir) %> 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
<%
  end
  ring_name = "collision"
  ring = rings[ring_name]
  box_angles.each_with_index do |a,i|
    name = "box_#{ring_name}_#{i}"
    pos_xy = ring[:radius] * Vector[Math.cos(a), Math.sin(a)]
%>
    <model name='<%= name %>'>
      <pose><%= a_to_s(pos_xy) %> 0.1 0 -0 0</pose>
      <link name='link_1'>
        <inertial>
          <mass><%= box[:mass] %></mass>
          <inertia>
            <ixx><%= box[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= box[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= box[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <pose>0 0 0  0 0 <%= a %></pose>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1>0 1 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <pose>0 0 0  0 0 <%= a %></pose>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
<%
  end
  ring_name = "link"
  ring = rings[ring_name]
  box_angles.each_with_index do |a,i|
    name = "box_#{ring_name}_#{i}"
    pos_xy = ring[:radius] * Vector[Math.cos(a), Math.sin(a)]
%>
    <model name='<%= name %>'>
      <pose><%= a_to_s(pos_xy) %> 0.1 0 -0 0</pose>
      <link name='link_1'>
        <pose>0 0 0  0 0 <%= a %></pose>
        <inertial>
          <mass><%= box[:mass] %></mass>
          <inertia>
            <ixx><%= box[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= box[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= box[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1>0 1 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
<%
  end
  ring_name = "model"
  ring = rings[ring_name]
  box_angles.each_with_index do |a,i|
    name = "box_#{ring_name}_#{i}"
    pos_xy = ring[:radius] * Vector[Math.cos(a), Math.sin(a)]
%>
    <model name='<%= name %>'>
      <pose><%= a_to_s(pos_xy) %> 0.1 0 -0 <%= a %></pose>
      <link name='link_1'>
        <inertial>
          <mass><%= box[:mass] %></mass>
          <inertia>
            <ixx><%= box[:ixx] %></ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy><%= box[:iyy] %></iyy>
            <iyz>0</iyz>
            <izz><%= box[:izz] %></izz>
          </inertia>
        </inertial>
        <collision name='collision'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>0</mu2>
                <fdir1>0 1 0</fdir1>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size><%= a_to_s(box[:size]) %></size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
<%
  end
%>
    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>-0.93396 -19.9626 11.7572 0 0.503643 1.6562</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>
  </world>
</sdf>
