<?xml version="1.0" ?>
<gazebo version="1.2">
  <model name="simple_arm">
    <link name="arm_base">
      <inertial>
        <pose>0 0 0.00099 0 0 0</pose>
        <inertia>
          <ixx>1.11</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>100.11</iyy>
          <iyz>0</iyz>
          <izz>1.01</izz>
        </inertia>
        <mass>101.0</mass>
      </inertial>
      <collision name="arm_base_geom">
        <pose>0 0 0.05 0 0 0</pose>
        <geometry>
          <box>
            <size>1.0 1.0 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="arm_base_geom_visual">
        <pose>0 0 0.05 0 0 0</pose>
        <geometry>
          <box>
            <size>1.0 1.0 0.1</size>
          </box>
        </geometry>
        <material>
          <script>Gazebo/Blue</script>
        </material>
      </visual>
      <collision name="arm_base_geom_arm_trunk">
        <pose>0 0 0.6 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_base_geom_arm_trunk_visual">
        <pose>0 0 0.6 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Red</script>
        </material>
      </visual>
    </link>
    <link name="arm_shoulder_pan">
      <pose>0 0 1.1 0 0 0</pose>
      <inertial>
        <pose>0.045455 0 0 0 0 0</pose>
        <inertia>
          <ixx>0.011</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0225</iyy>
          <iyz>0</iyz>
          <izz>0.0135</izz>
        </inertia>
        <mass>1.1</mass>
      </inertial>
      <collision name="arm_shoulder_pan_geom">
        <pose>0 0 0.05 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.1</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_shoulder_pan_geom_visual">
        <pose>0 0 0.05 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.1</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Yellow</script>
        </material>
      </visual>
      <collision name="arm_shoulder_pan_geom_arm_shoulder">
        <pose>0.55 0 0.05 0 0 0</pose>
        <geometry>
          <box>
            <size>1.0 0.05 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="arm_shoulder_pan_geom_arm_shoulder_visual">
        <pose>0.55 0 0.05 0 0 0</pose>
        <geometry>
          <box>
            <size>1.0 0.05 0.1</size>
          </box>
        </geometry>
        <material>
          <script>Gazebo/Yellow</script>
        </material>
      </visual>
    </link>
    <link name="arm_elbow_pan">
      <pose>1.05 0 1.1 0 0 0</pose>
      <inertial>
        <pose>0.0875 0 0.083333 0 0 0</pose>
        <inertia>
          <ixx>0.031</ixx>
          <ixy>0</ixy>
          <ixz>0.005</ixz>
          <iyy>0.07275</iyy>
          <iyz>0</iyz>
          <izz>0.04475</izz>
        </inertia>
        <mass>1.2</mass>
      </inertial>
      <collision name="arm_elbow_pan_geom">
        <pose>0 0 0.1 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.2</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_elbow_pan_geom_visual">
        <pose>0 0 0.1 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.2</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Red</script>
        </material>
      </visual>
      <collision name="arm_elbow_pan_geom_arm_elbow">
        <pose>0.3 0 0.15 0 0 0</pose>
        <geometry>
          <box>
            <size>0.5 0.03 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="arm_elbow_pan_geom_arm_elbow_visual">
        <pose>0.3 0 0.15 0 0 0</pose>
        <geometry>
          <box>
            <size>0.5 0.03 0.1</size>
          </box>
        </geometry>
        <material>
          <script>Gazebo/Yellow</script>
        </material>
      </visual>
      <collision name="arm_elbow_pan_geom_arm_wrist">
        <pose>0.55 0 0.15 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.3</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_elbow_pan_geom_arm_wrist_visual">
        <pose>0.55 0 0.15 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.3</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Red</script>
        </material>
      </visual>
    </link>
    <link name="arm_wrist_lift">
      <pose>1.6 0 1.05 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.001</izz>
        </inertia>
        <mass>0.1</mass>
      </inertial>
      <collision name="arm_wrist_lift_geom">
        <pose>0 0 0.5 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.03</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_wrist_lift_geom_visual">
        <pose>0 0 0.5 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.03</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Yellow</script>
        </material>
      </visual>
    </link>
    <link name="arm_wrist_roll">
      <pose>1.6 0 1.0 0 0 0</pose>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.001</izz>
        </inertia>
        <mass>0.1</mass>
      </inertial>
      <collision name="arm_wrist_roll_geom">
        <pose>0 0 0.025 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="arm_wrist_roll_geom_visual">
        <pose>0 0 0.025 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <script>Gazebo/Red</script>
        </material>
      </visual>
    </link>
    <joint name="arm_base_joint" type="revolute">
      <parent>world</parent>
      <child>arm_base</child>
      <axis>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
        </limit>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <joint name="arm_shoulder_pan_joint" type="revolute">
      <parent>world</parent>
      <child>arm_shoulder_pan</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <joint name="arm_elbow_pan_joint" type="revolute">
      <parent>arm_shoulder_pan</parent>
      <child>arm_elbow_pan</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <joint name="arm_wrist_lift_joint" type="prismatic">
      <parent>arm_elbow_pan</parent>
      <child>arm_wrist_lift</child>
      <axis>
        <limit>
          <lower>-0.8</lower>
          <upper>0.1</upper>
        </limit>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <joint name="arm_wrist_roll_joint" type="revolute">
      <parent>arm_wrist_lift</parent>
      <child>arm_wrist_roll</child>
      <axis>
        <limit>
          <lower>-2.999994</lower>
          <upper>2.999994</upper>
        </limit>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <!--
    <plugin name="simple_arm_plugin" filename="libSimpleArmPlugin.so">
      <joint1>arm_base_joint</joint1>
      <joint2>arm_shoulder_pan_joint</joint2>
      <joint3>arm_elbow_pan_joint</joint3>
      <joint4>arm_wrist_lift_joint</joint4>
      <joint5>arm_wrist_roll_joint</joint5>
      <joint1_pid>1 0.01 0.1</joint1_pid>
      <joint2_pid>1 0.01 0.1</joint2_pid>
      <joint3_pid>1 0.01 0.1</joint3_pid>
      <joint4_pid>20 0.05 0.5</joint4_pid>
      <joint5_pid>1 0.01 0.1</joint5_pid>
      <joint1_ilim>-1 1</joint1_ilim>
      <joint2_ilim>-1 1</joint2_ilim>
      <joint3_ilim>-1 1</joint3_ilim>
      <joint4_ilim>-10 10</joint4_ilim>
      <joint5_ilim>-1 1</joint5_ilim>
    </plugin>
    -->
        <!--  attaching the pr2 gripper to simple_arm 
    <include filename="pr2_gripper.model" model_pose="0.829 0.188 0.209325 0 1.57079633 0"/>
    <origin pose='-0.771000 0.188000 -0.790675 0.000000 -1.57079633 0.000000'/>
    -->
        <!--<include filename="pr2_gripper.model" model_pose="0.81 0.188 1.795 0 1.57079633 0" />
        <joint name="arm_gripper_joint" type="revolute">
          <parent>arm_wrist_roll</parent>
          <child>pr2_gripper::r_wrist_roll_link</child>
          <axis>
            <limit>
              <lower>0</lower>
              <upper>0</upper>
            </limit>
            <xyz>0 0 1</xyz>
          </axis>
        </joint>
-->
  </model>
</gazebo>
