<?xml version="1.0" ?>
<robot name="urdf_gazebo_extension_test">

  <gazebo>
    <!-- Explicitly declare that model is not static. -->
    <static>0</static>
  </gazebo>

  <link name="world"/>

  <joint name="jointw0" type="continuous">
    <origin rpy="0 0 1.57079632679" xyz="0 0 1.0"/>
    <axis xyz="1 0 0"/>  <!-- in child (link1) frame -->
    <parent link="world"/>
    <child link="link0"/>
    <dynamics damping="0.0" friction="0.0" />
  </joint>

  <link name="link0">
    <inertial>
      <mass value="1000"/>
      <origin rpy="0 0 0" xyz="0 0 -0.5"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1 0.1 1.0"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.5"/>
    </visual>
  </link>

  <joint name="joint01" type="continuous">
    <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
    <axis xyz="1 0 0"/>  <!-- in child (link1) frame -->
    <parent link="link0"/>
    <child link="link1"/>
    <dynamics damping="1.1111" friction="2.2222" />
  </joint>
  <gazebo reference="joint01">
    <cfmDamping>true</cfmDamping>
    <stopCfm>123</stopCfm>
    <stopErp>0.987</stopErp>
    <springReference>0.234</springReference>
    <springStiffness>0.567</springStiffness>
    <provideFeedback>1</provideFeedback>
    <implicitSpringDamper>false</implicitSpringDamper>
    <fudgeFactor>0.56789</fudgeFactor>
  </gazebo>

  <link name="link1">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.2 3.0 0.2"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 -1.5 0"/>
    </visual>
  </link>
  <gazebo reference="link1">
    <enable_wind>1</enable_wind>
    <gravity>0</gravity>
    <velocity_decay>
      <linear>0.1</linear>
      <angular>0.2</angular>
    </velocity_decay>
  </gazebo>

  <joint name="joint12" type="revolute">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
    <axis xyz="0 1 0"/>  <!-- in child (link1) frame -->
    <limit lower="0" upper="0" velocity="1000" effort="10000"/>
    <parent link="link1"/>
    <child link="link2"/>
  </joint>
  <gazebo reference="joint12">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>

  <link name="link2">
    <inertial>
      <mass value="200"/>
      <origin rpy="2 -3 -1.57079632679" xyz="0.2 0.4 1.0"/>
      <inertia ixx="5" ixy="0" ixz="0" iyy="6" iyz="0" izz="7"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.3 0.3 2.0"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 1.0"/>
    </visual>
  </link>
  <gazebo reference="link2">
    <enable_wind>0</enable_wind>
    <turnGravityOff>1</turnGravityOff>
    <dampingFactor>0.1</dampingFactor>
  </gazebo>

  <joint name="joint23" type="revolute">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="-0.5 0.5 2.5"/>
    <axis xyz="0 1 0"/>  <!-- in child (link1) frame -->
    <limit lower="0" upper="0" velocity="1000" effort="10000"/>
    <parent link="link2"/>
    <child link="link3"/>
  </joint>

  <link name="link3">
    <inertial>
      <mass value="400"/>
      <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
      <inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.3 0.4 0.5"/>
      </geometry>
      <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
    </visual>
  </link>

  <joint name="joint14" type="revolute">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
    <axis xyz="0 1 0"/>  <!-- in child (link1) frame -->
    <limit lower="0" upper="0" velocity="1000" effort="10000"/>
    <parent link="link1"/>
    <child link="link4"/>
  </joint>
  <link name="link4">
    <inertial>
      <mass value="400"/>
      <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
      <inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.3 0.4 0.5"/>
      </geometry>
      <origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
    </visual>
  </link>
  <gazebo reference="joint14">
    <implicitSpringDamper>true</implicitSpringDamper>
  </gazebo>

  <joint name="joint01a" type="continuous">
    <origin rpy="0 0 1.57079632679" xyz="0 0 -1"/>
    <axis xyz="1 0 0"/>  <!-- in child (link1) frame -->
    <parent link="link0"/>
    <child link="link1a"/>
  </joint>

  <link name="link1a">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.2 3.0 0.2"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 -1.5 0"/>
    </visual>
  </link>

  <!-- Test lumping of sensors with <pose> tags. -->

  <link name="linkSensorNoPose">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
  </link>
  <joint name="jointSensorNoPose" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
    <parent link="link0"/>
    <child link="linkSensorNoPose"/>
  </joint>
  <gazebo reference="linkSensorNoPose">
    <light name="lightNoPose" type="point"/>
    <projector name="projectorNoPose"/>
    <sensor name="sensorNoPose" type="camera">
      <update_rate>6.0</update_rate>
      <camera name="cam">
        <horizontal_fov>1.36869112579</horizontal_fov>
        <image>
          <width>1232</width>
          <height>1616</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

  <link name="linkSensorPose">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
  </link>
  <joint name="jointSensorPose" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
    <parent link="link0"/>
    <child link="linkSensorPose"/>
  </joint>
  <gazebo reference="linkSensorPose">
    <light name="lightPose" type="point">
      <pose>111 0 0 0 0 -1</pose>
    </light>
    <projector name="projectorPose">
      <pose>111 0 0 0 0 -1</pose>
    </projector>
    <sensor name="sensorPose" type="camera">
      <pose>111 0 0 0 0 -1</pose>
      <update_rate>6.0</update_rate>
      <camera name="cam">
        <horizontal_fov>1.36869112579</horizontal_fov>
        <image>
          <width>1232</width>
          <height>1616</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

  <link name="linkSensorPoseRelative">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
  </link>
  <joint name="jointSensorPoseRelative" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
    <parent link="link0"/>
    <child link="linkSensorPoseRelative"/>
  </joint>
  <gazebo reference="linkSensorPoseRelative">
    <light name="lightPoseRelative" type="point">
      <pose relative_to="link0">111 0 0 0 0 -1</pose>
    </light>
    <projector name="projectorPoseRelative">
      <pose relative_to="link0">111 0 0 0 0 -1</pose>
    </projector>
    <sensor name="sensorPoseRelative" type="camera">
      <pose relative_to="link0">111 0 0 0 0 -1</pose>
      <update_rate>6.0</update_rate>
      <camera name="cam">
        <horizontal_fov>1.36869112579</horizontal_fov>
        <image>
          <width>1232</width>
          <height>1616</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

  <link name="linkSensorPoseTwoLevel">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
  </link>
  <joint name="jointSensorPoseTwoLevel" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 222"/>
    <parent link="link0"/>
    <child link="linkSensorPoseTwoLevel"/>
  </joint>
  <link name="linkSensorPoseTwoLevel2">
    <inertial>
      <mass value="100"/>
      <origin rpy="1 3 4" xyz="0 -1.5 0"/>
      <inertia ixx="2" ixy="0" ixz="0" iyy="3" iyz="0" izz="4"/>
    </inertial>
  </link>
  <joint name="jointSensorPoseTwoLevel2" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="333 0 0"/>
    <parent link="linkSensorPoseTwoLevel"/>
    <child link="linkSensorPoseTwoLevel2"/>
  </joint>
  <gazebo reference="linkSensorPoseTwoLevel2">
    <light name="lightPoseTwoLevel" type="point">
      <!-- TwoLevel to link0 -->
      <pose>111 0 0 0 0 -1</pose>
    </light>
    <projector name="projectorPoseTwoLevel">
      <!-- TwoLevel to link0 -->
      <pose>111 0 0 0 0 -1</pose>
    </projector>
    <sensor name="sensorPoseTwoLevel" type="camera">
      <!-- TwoLevel to link0 -->
      <pose>111 0 0 0 0 -1</pose>
      <update_rate>6.0</update_rate>
      <camera name="cam">
        <horizontal_fov>1.36869112579</horizontal_fov>
        <image>
          <width>1232</width>
          <height>1616</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

  <!-- Issue 378 setting -->
  <gazebo reference="issue378_link">
    <light name="issue378_light" type="point">
      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
    </light>
    <projector name="issue378_projector">
      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
    </projector>
    <sensor name="issue378_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>400</update_rate>
      <pose>1.0 2.0 3.0 0.1 0.2 0.3</pose>
    </sensor>
  </gazebo>
  <link name="issue378_link"/>
  <joint name="issue378_link_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 -0 0"/>
    <axis xyz="0 0 0"/>
    <parent link="link0"/>
    <child link="issue378_link"/>
  </joint>

  <!-- Issue 67 setting -->
  <link name="Camera">
    <inertial>
      <origin xyz="-7.4418E-06 0.0043274 -0.010112" rpy="0 0 0"/>
      <mass value="0.10621"/>
      <inertia ixx="7.3134E-05" ixy="7.9651E-09" ixz="-8.9146E-09"
              iyy="3.0769E-05" iyz="-3.9082E-06"
              izz="9.2194E-05"/>
    </inertial>
  </link>
  <joint name="jCamera" type="fixed">
    <origin xyz="-0.20115 0.42488 0.30943" rpy="1.5708 -0.89012 1.5708"/>
    <parent link="link0"/>
    <child link="Camera"/>
    <axis xyz="0 0 0"/>
  </joint>
  <gazebo reference="Camera">
    <light name="issue67_light" type="point">
      <pose>1 1 1 1.570796 1.570796 1.570796</pose>
    </light>
    <projector name="issue67_projector">
      <pose>1 1 1 1.570796 1.570796 1.570796</pose>
    </projector>
    <sensor name="issue67_sensor" type="camera">
      <visualize>true</visualize>
      <pose>1 1 1 1.570796 1.570796 1.570796</pose>
      <camera>
      </camera>
    </sensor>
  </gazebo>

</robot>
