<?xml version="1.0"?>
<robot name="robotic_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find robotic_arm)/urdf/materials.xacro" />
    <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>
            <geometry>
                <cylinder radius="0.1" length="0.05" />
            </geometry>
            <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0" />
            <material name="grey" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.1" length="0.05" />
            </geometry>
            <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <mass value="1.0" />
            <origin xyz="0.0 0.0 0.025" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>
    </link>
    <joint name="joint1" type="revolute">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <parent link="base_link" />
        <child link="link1" />
        <axis xyz="0.0 0.0 1.0" />
        <limit lower="-3.14159" upper="3.14159" effort="100.0" velocity="1.0" />
     
        <dynamics damping="0.1" friction="0.1" />
    </joint>
    <link name="link1">
        <visual>
            <geometry>
                <box size="0.08 0.08 0.15" />
            </geometry>
            <origin xyz="0.0 0.0 0.075" rpy="0.0 0.0 0.0" />
            <material name="blue" />
        </visual>
        <collision>
            <geometry>
                <box size="0.08 0.08 0.15" />
            </geometry>
            <origin xyz="0.0 0.0 0.075" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <mass value="0.8" />
            <origin xyz="0 0 0.075" />
            <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.003" />
        </inertial>
    </link>
    <joint name="joint2" type="revolute">
        <parent link="link1" />
        <child link="link2" />
        <origin xyz="0.0 0.0 0.15" rpy="0.0 0.0 0.0" />
        <axis xyz="0.0 1.0 0.0" />
        <limit lower="-1.5708" upper="1.5708" effort="100.0" velocity="1.0" />
        
        <dynamics damping="0.1" friction="0.1" />
    </joint>
    <link name="link2">
        <visual>
            <geometry>
                <box size="0.06 0.06 0.20" />
            </geometry>
            <origin xyz="0.0 0.0 0.10" rpy="0.0 0.0 0.0" />
            <material name="orange" />
        </visual>
        <collision>
            <geometry>
                <box size="0.06 0.06 0.20" />
            </geometry>
            <origin xyz="0.0 0.0 0.10" rpy="0.0 0.0 0.0" />
        </collision>
        <inertial>
            <mass value="0.6" />
            <origin xyz="0 0 0.10" />
            <inertia ixx="0.003" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.002" />
        </inertial>
    </link>
    <joint name="joint3" type="revolute">
        <parent link="link2" />
        <child link="link3" />
        <origin xyz="0 0 0.20" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <limit lower="-1.5708" upper="1.5708" effort="80" velocity="1.0" />
        <dynamics damping="0.1" friction="0.1" />
    </joint>
    <link name="link3">
        <visual>
            <geometry>
                <box size="0.055 0.055 0.18" />
            </geometry>
            <origin xyz="0 0 0.09" rpy="0 0 0" />
            <material name="blue" />
        </visual>
        <collision>
            <geometry>
                <box size="0.055 0.055 0.18" />
            </geometry>
            <origin xyz="0 0 0.09" rpy="0 0 0" />
        </collision>
        <inertial>
            <mass value="0.5" />
            <origin xyz="0 0 0.09" />
            <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.001" />
        </inertial>
    </link>
    <joint name="joint4" type="revolute">
        <parent link="link3" />
        <child link="link4" />
        <origin xyz="0 0 0.18" rpy="0 0 0" />
        <axis xyz="0 0 1" />
        <limit lower="-3.14159" upper="3.14159" effort="50" velocity="1.5" />
        <dynamics damping="0.05" friction="0.05" />
    </joint>
    <link name="link4">
        <visual>
            <geometry>
                <box size="0.05 0.05 0.14" />
            </geometry>
            <origin xyz="0 0 0.07" rpy="0 0 0" />
            <material name="orange" />
        </visual>
        <collision>
            <geometry>
                <box size="0.05 0.05 0.14" />
            </geometry>
            <origin xyz="0 0 0.07" rpy="0 0 0" />
        </collision>
        <inertial>
            <mass value="0.3" />
            <origin xyz="0 0 0.07" />
            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
        </inertial>
    </link>
    <joint name="joint5" type="revolute">
        <parent link="link4" />
        <child link="link5" />
        <origin xyz="0 0 0.14" rpy="0 0 0" />
        <axis xyz="0 1 0" />
        <limit lower="-1.5708" upper="1.5708" effort="30" velocity="1.5" />
        <dynamics damping="0.05" friction="0.05" />
    </joint>
    <link name="link5">
        <visual>
            <geometry>
                <box size="0.045 0.045 0.10" />
            </geometry>
            <origin xyz="0 0 0.05" rpy="0 0 0" />
            <material name="blue" />
        </visual>
        <collision>
            <geometry>
                <box size="0.045 0.045 0.10" />
            </geometry>
            <origin xyz="0 0 0.05" rpy="0 0 0" />
        </collision>
        <inertial>
            <mass value="0.2" />
            <origin xyz="0 0 0.05" />
            <inertia ixx="0.0005" ixy="0" ixz="0" iyy="0.0005" iyz="0" izz="0.0003" />
        </inertial>
    </link>
    <joint name="joint6" type="revolute">
        <parent link="link5" />
        <child link="link6" />
        <origin xyz="0 0 0.10" rpy="0 0 0" />
        <axis xyz="0 0 1" />
        <limit lower="-3.14159" upper="3.14159" effort="20" velocity="2.0" />
        <dynamics damping="0.05" friction="0.05" />
    </joint>
    <link name="link6">
        <visual>
            <geometry>
                <cylinder radius="0.03" length="0.06" />
            </geometry>
            <origin xyz="0 0 0.03" rpy="0 0 0" />
            <material name="grey" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="0.03" length="0.06" />
            </geometry>
            <origin xyz="0 0 0.03" rpy="0 0 0" />
        </collision>
        <inertial>
            <mass value="0.1" />
            <origin xyz="0 0 0.03" />
            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
        </inertial>
    </link>
    <joint name="end_effector_fixed" type="fixed">
        <parent link="link6" />
        <child link="end_effector" />
        <origin xyz="0 0 0.06" rpy="0 0 0" />
    </joint>
    <link name="end_effector" />
    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find robotic_arm)/config/controllers.yaml</parameters>
        </plugin>
    </gazebo>
    <gazebo reference="base_link">
        <material>Gazebo/Grey</material>
    </gazebo>
    <gazebo reference="link1">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link2">
        <material>Gazebo/Orange</material>
    </gazebo>
    <gazebo reference="link3">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link4">
        <material>Gazebo/Orange</material>
    </gazebo>
    <gazebo reference="link5">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link6">
        <material>Gazebo/Grey</material>
    </gazebo>

    <ros2_control name="robotic_arm" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>

        <joint name="joint1">
            <command_interface name="position">
                <param name="min">-3.14159</param>
                <param name="max">3.14159</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>

        <joint name="joint2">
            <command_interface name="position">
                <param name="min">-1.5708</param>
                <param name="max">1.5708</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>

        <joint name="joint3">
            <command_interface name="position">
                <param name="min">-1.5708</param>
                <param name="max">1.5708</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>

        <joint name="joint4">
            <command_interface name="position">
                <param name="min">-3.14159</param>
                <param name="max">3.14159</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>

        <joint name="joint5">
            <command_interface name="position">
                <param name="min">-1.5708</param>
                <param name="max">1.5708</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>

        <joint name="joint6">
            <command_interface name="position">
                <param name="min">-3.14159</param>
                <param name="max">3.14159</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
        </joint>
    </ros2_control>
</robot>