<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="six_axis_arm">

  <xacro:property name="PI" value="3.1415926535897931"/>
  
  <xacro:property name="LENGTH_SCALE" value="1.6"/>
  
  <material name="grey"><color rgba="0.2 0.2 0.2 1.0"/></material>
  <material name="orange"><color rgba="1.0 0.42 0.04 1.0"/></material>
  <material name="blue"><color rgba="0.05 0.35 0.65 1.0"/></material>

  <xacro:macro name="cylinder_inertial" params="mass radius length *origin">
    <inertial>
      <xacro:insert_block name="origin"/>
      <mass value="${mass}"/>
      <inertia 
        ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
        iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
        izz="${(1/2) * mass * radius * radius}"/>
    </inertial>
  </xacro:macro>

  <!-- <link name="world"/>

  <joint name="world_to_base" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint> -->

  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><cylinder radius="0.15" length="0.1"/></geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><cylinder radius="0.15" length="0.1"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="5.0" radius="0.15" length="0.1">
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-${PI}" upper="${PI}" effort="300.0" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="link_1">
    <visual>
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.08" length="${0.3 * LENGTH_SCALE}"/></geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.08" length="${0.3 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="4.0" radius="0.08" length="${0.3 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_2" type="revolute">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin xyz="0 0 ${0.3 * LENGTH_SCALE}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.0" upper="2.0" effort="300.0" velocity="2.0"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="link_2">
    <visual>
      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.07" length="${0.4 * LENGTH_SCALE}"/></geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.07" length="${0.4 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="3.5" radius="0.07" length="${0.4 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_3" type="revolute">
    <parent link="link_2"/>
    <child link="link_3"/>
    <origin xyz="0 0 ${0.4 * LENGTH_SCALE}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5" upper="2.5" effort="200.0" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
  </joint>

  <link name="link_3">
    <visual>
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.06" length="${0.3 * LENGTH_SCALE}"/></geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.06" length="${0.3 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="2.5" radius="0.06" length="${0.3 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_4" type="revolute">
    <parent link="link_3"/>
    <child link="link_4"/>
    <origin xyz="0 0 ${0.3 * LENGTH_SCALE}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-${PI}" upper="${PI}" effort="100.0" velocity="3.0"/>
    <dynamics damping="0.5" friction="0.5"/>
  </joint>

  <link name="link_4">
    <visual>
      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.05" length="${0.2 * LENGTH_SCALE}"/></geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.05" length="${0.2 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="1.8" radius="0.05" length="${0.2 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_5" type="revolute">
    <parent link="link_4"/>
    <child link="link_5"/>
    <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.0" upper="2.0" effort="100.0" velocity="3.0"/>
    <dynamics damping="0.5" friction="0.5"/>
  </joint>

  <link name="link_5">
    <visual>
      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.04" length="${0.1 * LENGTH_SCALE}"/></geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.04" length="${0.1 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="1.0" radius="0.04" length="${0.1 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <joint name="joint_6" type="revolute">
    <parent link="link_5"/>
    <child link="link_6"/>
    <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-${PI}" upper="${PI}" effort="50.0" velocity="4.0"/>
    <dynamics damping="0.2" friction="0.2"/>
  </joint>

  <link name="link_6">
    <visual>
      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.03" length="${0.04 * LENGTH_SCALE}"/></geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
      <geometry><cylinder radius="0.03" length="${0.04 * LENGTH_SCALE}"/></geometry>
    </collision>
    <xacro:cylinder_inertial mass="0.5" radius="0.03" length="${0.04 * LENGTH_SCALE}">
      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
    </xacro:cylinder_inertial>
  </link>

  <link name="tool_link"/>
  <joint name="tool_joint" type="fixed">
    <parent link="link_6"/>
    <child link="tool_link"/>
    <origin xyz="0 0 ${0.04 * LENGTH_SCALE}" rpy="0 0 0"/>
  </joint>

  <ros2_control name="GazeboSimSystem" type="system">
    <hardware>
      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
    </hardware>
    
    <joint name="joint_1">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint_2">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint_3">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint_4">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint_5">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>

    <joint name="joint_6">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

  <gazebo>
    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
      <parameters>$(find robotarm_description)/config/ros2_controllers.yaml</parameters>
    </plugin>
  </gazebo>

</robot>