<?xml version="1.0"?>
<robot name="continuous_pendulum"
       xmlns:xi="http://www.w3.org/2001/XInclude"
       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:xacro="http://ros.org/wiki/xacro"
       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">

  <link name="world"/>
  <!--
     A pendulum constructed from single continuous link and a mass with some moment of inertia
  -->
  <xacro:macro name="one_link" params="link_name parent *axis *origin material">
    <joint name="${link_name}_joint" type="continuous" >
      <insert_block name="origin"/>
      <insert_block name="axis"/>
      <parent link="${parent}" />
      <child link="${link_name}" />
      <dynamics damping="0.0" />
    </joint>
    <link name="${link_name}">
      <inertial >
        <mass value="1.0" />
        <origin xyz="0 1 0" />
        <inertia  ixx="0.0" ixy="0.0"  ixz="0.0"  iyy="0.0"  iyz="0.0"  izz="0.0" />
      </inertial>
      <visual >
        <origin xyz="0 0.5 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 1.0 0.2" />
        </geometry>
      </visual>
      <collision >
        <origin xyz="0 0.5 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 1.0 0.2" />
        </geometry>
      </collision>
    </link>
    <gazebo reference="${link_name}">
      <material>${material}</material>
      <turnGravityOff>false</turnGravityOff>
    </gazebo>
    <transmission type="SimpleTransmission" name="${link_name}_trans">
      <actuator name="${link_name}_motor" />
      <joint name="${link_name}_joint" />
      <mechanicalReduction>1</mechanicalReduction>
      <motorTorqueConstant>1</motorTorqueConstant>
    </transmission>
    <gazebo>
      <controller:gazebo_ros_p3d name="p3d_${link_name}_controller" plugin="libgazebo_ros_p3d.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>100.0</updateRate>
          <bodyName>${link_name}</bodyName>
          <topicName>${link_name}_pose</topicName>
          <frameName>map</frameName>
      </controller:gazebo_ros_p3d>
    </gazebo>
  </xacro:macro>
  <xacro:macro name="pendulum" params="chain_num x_offset">
    <xacro:one_link link_name="link1_${chain_num}" parent="world" material="Gazebo/GrassFloor">
      <origin xyz="${x_offset} 0 15" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link2_${chain_num}" parent="link1_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link3_${chain_num}" parent="link2_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link4_${chain_num}" parent="link3_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link5_${chain_num}" parent="link4_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link6_${chain_num}" parent="link5_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link7_${chain_num}" parent="link6_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link8_${chain_num}" parent="link7_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link9_${chain_num}" parent="link8_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link10_${chain_num}" parent="link9_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link11_${chain_num}" parent="link10_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link12_${chain_num}" parent="link11_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link13_${chain_num}" parent="link12_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link14_${chain_num}" parent="link13_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link15_${chain_num}" parent="link14_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link16_${chain_num}" parent="link15_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link17_${chain_num}" parent="link16_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link18_${chain_num}" parent="link17_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link19_${chain_num}" parent="link18_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link20_${chain_num}" parent="link19_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link21_${chain_num}" parent="link20_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link22_${chain_num}" parent="link21_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link23_${chain_num}" parent="link22_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link24_${chain_num}" parent="link23_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link25_${chain_num}" parent="link24_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link26_${chain_num}" parent="link25_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link27_${chain_num}" parent="link26_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link28_${chain_num}" parent="link27_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link29_${chain_num}" parent="link28_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link30_${chain_num}" parent="link29_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link31_${chain_num}" parent="link30_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link32_${chain_num}" parent="link31_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link33_${chain_num}" parent="link32_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link34_${chain_num}" parent="link33_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link35_${chain_num}" parent="link34_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link36_${chain_num}" parent="link35_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link37_${chain_num}" parent="link36_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link38_${chain_num}" parent="link37_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link39_${chain_num}" parent="link38_${chain_num}" material="Gazebo/LightWood">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
    <xacro:one_link link_name="link40_${chain_num}" parent="link39_${chain_num}" material="Gazebo/GrassFloor">
      <origin xyz="0 1 0" rpy="0 0 0"/>
      <axis xyz="1 0 0" />
    </xacro:one_link>
  </xacro:macro>

  <xacro:pendulum x_offset="0" chain_num="0" />
  <xacro:pendulum x_offset="1" chain_num="1" />
  <xacro:pendulum x_offset="2" chain_num="2" />
  <xacro:pendulum x_offset="3" chain_num="3" />
  <xacro:pendulum x_offset="4" chain_num="4" />
  <xacro:pendulum x_offset="5" chain_num="5" />
  <xacro:pendulum x_offset="6" chain_num="6" />
  <xacro:pendulum x_offset="7" chain_num="7" />
  <xacro:pendulum x_offset="8" chain_num="8" />
  <xacro:pendulum x_offset="9" chain_num="9" />
  <xacro:pendulum x_offset="10" chain_num="10" />
  <xacro:pendulum x_offset="11" chain_num="11" />
  <xacro:pendulum x_offset="12" chain_num="12" />
  <xacro:pendulum x_offset="13" chain_num="13" />
  <xacro:pendulum x_offset="14" chain_num="14" />
  <xacro:pendulum x_offset="15" chain_num="15" />
  <xacro:pendulum x_offset="16" chain_num="16" />
  <xacro:pendulum x_offset="17" chain_num="17" />
  <xacro:pendulum x_offset="18" chain_num="18" />
  <xacro:pendulum x_offset="19" chain_num="19" />
  <xacro:pendulum x_offset="20" chain_num="20" />
  <xacro:pendulum x_offset="22" chain_num="21" />
  <xacro:pendulum x_offset="22" chain_num="22" />
  <xacro:pendulum x_offset="23" chain_num="23" />
  <xacro:pendulum x_offset="24" chain_num="24" />
  <xacro:pendulum x_offset="25" chain_num="25" />
  <xacro:pendulum x_offset="26" chain_num="26" />
  <xacro:pendulum x_offset="27" chain_num="27" />
  <xacro:pendulum x_offset="28" chain_num="28" />
  <xacro:pendulum x_offset="29" chain_num="29" />

  <gazebo>
      <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>1000.0</updateRate>
        <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
      </controller:gazebo_ros_controller_manager>
  </gazebo>

</robot>
