<?xml version="1.0"?>
<robot
    name="skyentific_robot"
    xmlns:xacro="http://www.ros.org/wiki/xacro"
>
    <!-- Include ros2_control Plugins -->
    <xacro:include
        filename="$(find robot_description)/urdf/skyentific_robot_ros2_control.xacro"
    />
    <!-- Useful XACRO Variables (Properties) -->
    <xacro:property name="PI" value="3.14159265359" />
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
    </material>
    <material name="red">
        <color rgba="0.5 0.0 0.0 1.0"/>
    </material>

    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/base_z_ob.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey" />
        </visual>
        <collision> <!-- cyclinder equivalent -->
            <origin xyz="0 0 0.075" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.1" length="0.15"/>
            </geometry>
        </collision>
    </link>

    <link name="link_1">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/link_1.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey" />
        </visual>
        <collision>
            <origin xyz="0 0 0.150" rpy="1.57 0 0"/>
            <geometry>
                <cylinder radius="0.095" length="0.1"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_1" type="revolute">
        <parent link="base_link"/>
        <child link="link_1"/>
        <origin xyz="0 0 0.0965" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-3.14"
            upper="3.14"
        />
    </joint>

    <link name="link_2">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/link_2.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey"/>
        </visual>
        <collision>
            <origin xyz="0.225 0 0" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.07" length="0.1"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_2" type="revolute">
        <parent link="link_1"/>
        <child link="link_2"/>
        <origin xyz="0 0 0.150" rpy="0 -1.57 1.57"/>
        <axis xyz="0 0 1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-2.51"
            upper="2.51"
        />
    </joint>

    <link name="link_3">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/link_3.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey" />
        </visual>
        <collision>
            <origin xyz="0.2 0 0" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.055" length="0.1"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_3" type="revolute">
        <parent link="link_2"/>
        <child link="link_3"/>
        <origin xyz="0.225 0 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-2.51"
            upper="2.51"
        />
    </joint>

    <link name="link_4">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/link_4.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey" />
        </visual>
        <collision>
            <origin xyz="0.1 0 0" rpy="0 1.57 0"/>
            <geometry>
                <cylinder radius="0.055" length="0.06"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_4" type="revolute">
        <parent link="link_3"/>
        <child link="link_4"/>
        <origin xyz="0.2 0 0" rpy="0 0 0"/>
        <axis xyz="0 0 1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-2.51"
            upper="2.51"
        />
    </joint>

    <link name="link_5">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/link_5_gripper.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="grey" />
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="1.57 0 0"/>
            <geometry>
                <cylinder radius="0.055" length="0.01"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_5" type="revolute">
        <parent link="link_4"/>
        <child link="link_5"/>
        <origin xyz="0.0915 0 0" rpy="0 1.57 0"/>
        <axis xyz="0 0 1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-2.51"
            upper="2.51"
        />
    </joint>

    <link name="gripper_right">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/rightjaw.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="red"/>
        </visual>
        <collision>
            <origin xyz="0.01 0.025 0.045" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.0125" length="0.04"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_6" type="prismatic">
        <parent link="link_5"/>
        <child link="gripper_right"/>
        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-0.018"
            upper="0.02"
        />
    </joint>

    <link name="gripper_left">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <mesh
                    filename="file://$(find robot_description)/meshes/leftjaw.stl"
                    scale="0.001 0.001 0.001"
                />
            </geometry>
            <material name="red"/>
        </visual>
        <collision>
            <origin xyz="0.01 -0.025 0.045" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.0125" length="0.04"/>
            </geometry>
        </collision>
    </link>

    <joint name="joint_7" type="prismatic">
        <parent link="link_5"/>
        <child link="gripper_left"/>
        <origin xyz="-0.01 0 0.1015" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <mimic joint="joint_6" multiplier="-1"/>
        <limit
            effort="1000.0"
            velocity="100.0"
            lower="-0.018"
            upper="0.02"
        />
    </joint>

</robot>
