<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <physics type="ode">
      <gravity>1 0 -9.81</gravity>
      <max_step_size>0.00101</max_step_size>
    </physics>
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="double_pendulum">
      <%
        # Parameters
        # Aluminum 2700 kg/m^3
        density = 2700.0
        dz = 0.5
        dx = dz/10
        dy = dz/10
        mass = density * dx * dy * dz 
        # upper_link is smaller
        length_ratio = 2e-1
        dx2 = dx*length_ratio
        dy2 = dy*length_ratio
        dz2 = dz*length_ratio
        mass2 = density * dx2 * dy2 * dz2 
        theta_1 = Math::PI / 4
        theta_2 = 0.3
      %>
      <pose>0 0 <%= dz*1.8 %>  <%= theta_1 %> 0 0</pose>
      <link name="upper_link">
        <pose>0 0 <%= -dz2/2 %>  0 0 0</pose>
        <inertial>
          <mass><%= mass2 %></mass>
          <inertia>
            <ixx><%= 1.0 / 12 * mass2 * (dy2*dy2 + dz2*dz2) %></ixx>
            <iyy><%= 1.0 / 12 * mass2 * (dz2*dz2 + dx2*dx2) %></iyy>
            <izz><%= 1.0 / 12 * mass2 * (dx2*dx2 + dy2*dy2) %></izz>
            <ixy>0.0</ixy>
            <ixz>0.0</ixz>
            <iyz>0.0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size><%= dx2 %> <%= dy2 %> <%= dz2 %></size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size><%= dx2 %> <%= dy2 %> <%= dz2 %></size>
            </box>
          </geometry>
        </visual>
      </link>
      <joint name="upper_joint" type="revolute">
        <pose>0 0 <%= dz2/2 %>  0 0 0</pose>
        <parent>world</parent>
        <child>upper_link</child>
        <axis>
          <xyz>1 0 0</xyz>
        </axis>
      </joint>
      <link name="lower_link">
        <pose>0 <%= dz2*Math.sin(0)+dz*Math.sin(theta_1)/2 %> <%= -dz2*Math.cos(0)-dz*Math.cos(theta_1)/2 %>  <%= theta_1 %> 0 0</pose>
        <inertial>
          <mass><%= mass %></mass>
          <inertia>
            <ixx><%= 1.0 / 12 * mass * (dy*dy + dz*dz) %></ixx>
            <iyy><%= 1.0 / 12 * mass * (dz*dz + dx*dx) %></iyy>
            <izz><%= 1.0 / 12 * mass * (dx*dx + dy*dy) %></izz>
            <ixy>0.0</ixy>
            <ixz>0.0</ixz>
            <iyz>0.0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size><%= dx %> <%= dy %> <%= dz %></size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size><%= dx %> <%= dy %> <%= dz %></size>
            </box>
          </geometry>
        </visual>
      </link>
      <joint name="lower_joint" type="revolute">
        <pose>0 0 <%= dz/2 %>  0 0 0</pose>
        <parent>upper_link</parent>
        <child>lower_link</child>
        <axis>
          <xyz>1 0 0</xyz>
        </axis>
      </joint>
    </model>
  </world>
</sdf>
