<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arduinobot">
    
    <!-- Useful XACRO Variables (Properties) -->
    <xacro:property name="PI" value="3.14159265359" />
    <xacro:property name="effort" value="30.0" />
    <xacro:property name="velocity" value="10.0" />

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

    <link name="base_link">
        <visual>
            <origin rpy="0 0 0" xyz="-0.5 -0.5 0"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/basement.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="base_plate">
        <visual>
            <origin rpy="0 0 0" xyz="-0.39 -0.39 -0.56"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/base_plate.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="forward_drive_arm">
        <visual>
            <origin rpy=" 0 -${PI / 2} ${PI / 2}" xyz="0.19 0.06 -0.08"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/forward_drive_arm.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="horizontal_arm">
        <visual>
            <origin rpy="${PI / 2} 0 ${PI / 2}" xyz="-0.03 -0.4 -0.06"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/horizontal_arm.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="claw_support">
        <visual>
            <origin rpy="0 0 ${PI / 2}" xyz="0 -0.05 -0.15"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/claw_support.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="gripper_right">
        <visual>
            <origin rpy="0 0 -${PI / 2}" xyz="-0.1 0.50 -0.1"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/right_finger.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="gripper_left">
        <visual>
            <origin rpy="0 0 -${PI / 2}" xyz="-0.04 0.50 -0.1"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/left_finger.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <link name="rgb_camera">
        <visual>
            <origin rpy="-1.57 0 -1.57" xyz="-0.1 0.125 0.15"/>
            <geometry>
                <mesh filename="package://arduinobot_description/meshes/pi_camera.STL" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

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

    <joint name ="joint_1" type="revolute">
        <parent link="base_link"/>
        <child link = "base_plate"/>
        <origin xyz="0 0 0.307"/>
        <axis xyz="0 0 1"/>
        <limit lower="-${PI / 2}" upper="${PI / 2}" 
               effort="${effort}" velocity="${velocity}"/>
    </joint>

    <joint name ="joint_2" type="revolute">
        <parent link="base_plate"/>
        <child link = "forward_drive_arm"/>
        <origin xyz="-0.02 0 0.35"/>
        <axis xyz="1 0 0"/>
        <limit lower="-${PI / 2}" upper="${PI / 2}"
                effort="${effort}" velocity="${velocity}"/>
    </joint>

    <joint name ="joint_3" type="revolute">
        <parent link="forward_drive_arm"/>
        <child link = "horizontal_arm"/>
        <origin xyz="0 0 0.8"/>
        <axis xyz="1 0 0"/>
        <limit lower="-${PI / 2}" upper="${PI / 2}"
               effort="${effort}" velocity="${velocity}"/>
    </joint>

    <joint name ="horizontal_arm_to_claw_support" type="fixed">
        <parent link="horizontal_arm"/>
        <child link = "claw_support"/>
        <origin xyz="0 0.82 0"/>
    </joint>

    <joint name ="joint_4" type="revolute">
        <parent link="claw_support"/>
        <child link = "gripper_right"/>
        <origin xyz="-0.04 0.13 -0.1"/>
        <axis xyz="0 0 1"/>
        <limit lower="-${PI / 2}" upper="0.0" 
               effort="${effort}" velocity="${velocity}"/>
    </joint>

    <joint name ="joint_5" type="revolute">
        <parent link="claw_support"/>
        <child link = "gripper_left"/>
        <origin xyz="-0.22 0.13 -0.1"/>
        <axis xyz="0 0 1"/>
        <limit lower="0.0" upper="${PI / 2}" 
               effort="${effort}" velocity="${velocity}"/>
        <mimic joint="joint_4" multiplier="-1"/>
    </joint>

    <joint name ="rgb_camera_joint" type="fixed">
        <parent link="base_link"/>
        <child link = "rgb_camera"/>
        <origin xyz="0 0.45 0.2" rpy="0 -0.5 1.57"/>
    </joint>

</robot>
