<?xml version="1.0"?>
<robot name="chain_test">
  <!-- Material definition -->
  <material name="Grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>

  <!-- Base link -->
  <link name="base">
    <inertial>
      <mass value="10.0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <!-- Chain 1: base -> fixed_1 -> fixed_2 -> fixed_3 -> revolute -> arm -->
  <joint name="fix_j1" type="fixed">
    <origin xyz="0.1 0 0" rpy="0.3 0 0"/>
    <parent link="base"/>
    <child link="fixed_1"/>
  </joint>
  <link name="fixed_1">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.02" length="0.05"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <joint name="fix_j2" type="fixed">
    <origin xyz="0.1 0 0" rpy="0 0.4 0"/>
    <parent link="fixed_1"/>
    <child link="fixed_2"/>
  </joint>
  <link name="fixed_2">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.025" length="0.05"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <joint name="fix_j3" type="fixed">
    <origin xyz="0.1 0 0" rpy="0 0 0.5"/>
    <parent link="fixed_2"/>
    <child link="fixed_3"/>
  </joint>
  <link name="fixed_3">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.05"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="3.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <joint name="shoulder" type="revolute">
    <origin xyz="0.4 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="fixed_3"/>
    <child link="arm"/>
    <limit lower="-3.14" upper="3.14" effort="100.0" velocity="2.0"/>
  </joint>
  <link name="arm">
    <visual>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.5"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="5.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <!-- Chain 2: arm -> adapter_1 -> adapter_2 -> prismatic -> slider -->
  <joint name="adapt_j1" type="fixed">
    <origin xyz="0 0 0.5" rpy="0.1 0.0 0.2"/>
    <parent link="arm"/>
    <child link="adapter_1"/>
  </joint>
  <link name="adapter_1">
    <visual>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.1"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <joint name="adapt_j2" type="fixed">
    <origin xyz="0 0 0.1" rpy="0 0.3 0"/>
    <parent link="adapter_1"/>
    <child link="adapter_2"/>
  </joint>
  <link name="adapter_2">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <joint name="slider" type="prismatic">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="adapter_2"/>
    <child link="end_effector"/>
    <limit lower="0.0" upper="0.5" effort="50.0" velocity="1.0"/>
  </joint>
  <link name="end_effector">
    <visual>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <geometry>
        <box size="0.06 0.06 0.2"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.005"/>
    </inertial>
  </link>
</robot>