<?xml version="1.0"?>

<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find xacro)/test/robots/pr2/head_v0/head.gazebo.xacro" />
  <xacro:include filename="$(find xacro)/test/robots/pr2/head_v0/head.transmission.xacro" />

  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="pr2_head_pan_v0" params="name parent *origin">
    <joint name="${name}_joint" type="revolute">
      <axis xyz="0 0 1" />
      <limit lower="-3.007" upper="3.007" effort="2.645" velocity="6" /> <!-- alpha tested velocity and effort limits -->
      <safety_controller k_velocity="1.5" soft_lower_limit="${-3.007+0.15}" soft_upper_limit="${3.007-0.15}" k_position="100" />
      <calibration rising="0.0" />
      <dynamics damping="0.5" />
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>
    <link name="${name}_link">
      <inertial>
        <mass value="6.339" /> <!-- mass/cog/moi updated per 100505_link_data.xls -->
        <origin xyz="0.010907 0.031693 0.090507" rpy="0 0 0" />
        <inertia ixx="0.032497592"  ixy="0.00063604088"  ixz="0.0025851534"
                 iyy="0.046545627"  iyz="-0.0024534295"  izz="0.057652724" />
      </inertial>

      <visual>
        <origin xyz="0 0 0.0" rpy="0 0 0 " />
        <geometry>
          <mesh filename="package://pr2_description/meshes/head_v0/head_pan.dae"  />
        </geometry>

        <material name="Blue" />
      </visual>

      <collision>
        <geometry>
          <mesh filename="package://pr2_description/meshes/head_v0/head_pan_L.stl"  />
        </geometry>
      </collision>

    </link>

    <!-- extensions -->
    <xacro:pr2_head_pan_transmission_v0 name="${name}" />
  </xacro:macro>



  <xacro:macro name="pr2_head_tilt_v0" params="name parent *origin">

    <joint name="${name}_joint" type="revolute">
      <axis xyz="0 1 0" />
      <limit lower="-0.471238" upper="1.39626" effort="18" velocity="5" /> <!-- alpha tested velocity and effort limits -->
      <safety_controller k_velocity="3.0" soft_lower_limit="${-0.4712 + 0.1}" soft_upper_limit="${1.39626 - 0.1}" k_position="100" />
      <calibration falling="${0.0+cal_head_tilt_flag}" />
      <dynamics damping="10.0" />
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>
    <link name="${name}_link">
      <inertial>
        <mass value="4.479" /> <!-- mass/cog/moi updated per 100505_link_data.xls -->
        <origin xyz="0.001716 -0.019556 0.055002" rpy="0 0 0" />
        <inertia ixx="0.024223222"  ixy="0.00062063507"  ixz="-0.000096909696"
                 iyy="0.054723086"  iyz="0.00279702400"
                 izz="0.067306377" />
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
        <geometry>
          <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.dae"  />
        </geometry>

        <material name="Green" />
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
        <geometry>
          <mesh filename="package://pr2_description/meshes/head_v0/head_tilt_L.stl"  />
        </geometry>
      </collision>
    </link>

    <!-- extensions -->
    <xacro:pr2_head_tilt_transmission_v0 name="${name}" />
  </xacro:macro>



  <xacro:macro name="pr2_head_v0" params="name parent *origin">
    <xacro:pr2_head_pan_v0 name="${name}_pan" parent="${parent}">
      <xacro:insert_block name="origin" />
    </xacro:pr2_head_pan_v0>

    <xacro:pr2_head_tilt_v0 name="${name}_tilt" parent="${name}_pan_link">
      <origin xyz="0.068 0 0" rpy="0 0 0" />
    </xacro:pr2_head_tilt_v0>

    <!-- Head plate frame -->
    <joint name="${name}_plate_frame_joint" type="fixed">
      <origin xyz="0.0232 0 0.0645" rpy="0 0 0" />
      <parent link="${name}_tilt_link"/>
      <child link="${name}_plate_frame"/>
    </joint>
    <link name="${name}_plate_frame">

      <inertial>
        <mass value="0.01" />
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
                 iyy="0.001"  iyz="0.0"
                 izz="0.001" />
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>

        <material name="Blue" />
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.01 0.01 0.01" />
        </geometry>
      </collision>
    </link>

    <!-- extensions -->
    <xacro:pr2_head_gazebo_v0 name="${name}" />

  </xacro:macro>


</robot>
