<?xml version="1.0" ?>
<sdf version="1.4">
  <model name="breakable_test">
    <pose>0 0 1  0 0 0</pose>
    <%
      # Parameters
      # Aluminum 2700 kg/m^3
      density = 2700.0
      dx = 0.05
      dy = dx
      dz = dx
      mass = density * dx * dy * dz 
      Lx = 0.5
      Lz = 0.5
      Nx = (Lx / dx).floor
      Nz = (Lz / dz).floor
    %>
    <%
      Nx.times do |xi|
        Nz.times do |zi|
    %>
      <%= "<link name=" + '"link_' + xi.to_s + '_' + zi.to_s + '">' %>
        <pose><%= xi*dx %> 0 <%= zi*dy %>  0 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=" + '"joint_' + xi.to_s + '_' + zi.to_s + '" type="revolute">' %>
        <parent>world</parent>
        <child>link_<%= xi %>_<%= zi %></child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>0.0</lower>
            <upper>0.0</upper>
          </limit>
        </axis>
        <physics>
          <ode>
            <erp>1</erp>
            <cfm>1</cfm>
          </ode>
        </physics>
        <sensor name="force_torque" type="force_torque">
          <always_on>true</always_on>
          <update_rate>1000</update_rate>
          <plugin name="breakable_<%= xi %>_<%= zi %>" filename="libBreakableJointPlugin.so">
            <breaking_force_N>50</breaking_force_N>
          </plugin>
        </sensor>
      </joint>
    <%
        end
      end
    %>
  </model>
</sdf>