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

    <!-- Definirea bazei si suportilor -->
    <link name="base_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Corp_hexapod.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/suport_servo_base1.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/suport_servo_base2.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/suport_servo_base3.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/suport_servo_base4.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/suport_servo_base5.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>

    </link>

    <!-- Definirea servomotoarelor montate la unghiuri -->
    <joint name="base_to_servo1" type="fixed">
    	<origin xyz="0.675 0.525 0.34" rpy="4.71 4.71 0.739"/> <!-- -->
    	<parent link="base_link"/>
    	<child link="servo1_link"/>
	</joint>
    <link name="servo1_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_1.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_to_servo2" type="fixed">
    	<origin xyz="0.675 -0.525 0.34" rpy="4.71 4.71 5.55"/> <!--  -->
    	<parent link="base_link"/>
    	<child link="servo2_link"/>
	</joint>
    <link name="servo2_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_2.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_to_servo3" type="fixed">
    	<origin xyz="-0.675 -0.525 0.34" rpy="4.71 4.71 -8.69"/> <!-- -->
    	<parent link="base_link"/>
    	<child link="servo3_link"/>
	</joint>
    <link name="servo3_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_3.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_to_servo4" type="fixed">
    	<origin xyz="-0.675 0.525 0.34" rpy="4.71 4.71 -10.16"/> <!-- -->
    	<parent link="base_link"/>
    	<child link="servo4_link"/>
	</joint>
    <link name="servo4_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_4.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_to_servo5" type="fixed">
    	<origin xyz="0.1 0.75 0.34" rpy="4.71 4.71 1.565"/> <!-- -->
    	<parent link="base_link"/>
    	<child link="servo5_link"/>
	</joint>
    <link name="servo5_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_5.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_to_servo6" type="fixed">
    	<origin xyz="0.1 -0.75 0.34" rpy="4.71 4.71 4.71"/> <!-- -->
    	<parent link="base_link"/>
    	<child link="servo6_link"/>
	</joint>
    <link name="servo6_link">
        <visual>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Servo_p_6.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <!-- Bratele de baza + link uri adiacente pentru axurile motoarelor-->
   

    <joint name="brat1_to_servo1" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo1_base_link"/>
        <child link="brat1_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
        <!-- New Link: servo1_base_link -->
    <link name="servo1_base_link">
        <visual>
            <origin xyz="0 0.5 0" rpy="0 0 0"/>
            <geometry>
                <cylinder radius="0.01" length="0.02"/> <!-- A small cylinder for visualization -->
            </geometry>
            <material name="blue">
                <color rgba="0 0 1 1"/> <!-- Optional: Blue color -->
            </material>
        </visual>
    </link>
    <link name="brat1_link">
        <visual>
            <origin xyz = "0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_1.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>


    <joint name="brat2_to_servo2" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo2_link"/>
        <child link="brat2_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="brat2_link">
        <visual>
            <origin xyz = "-0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_2.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="brat3_to_servo3" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo3_link"/>
        <child link="brat3_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="brat3_link">
        <visual>
            <origin xyz = "0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_3.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="brat4_to_servo4" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo4_link"/>
        <child link="brat4_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="brat4_link">
        <visual>
            <origin xyz = "-0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_4.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="brat5_to_servo5" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo5_link"/>
        <child link="brat5_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="brat5_link">
        <visual>
            <origin xyz = "-0.1 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_5.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>
_
    <joint name="brat6_to_servo6" type="revolute">
        <origin xyz="0 0 0" rpy="0 1.57 0"/>    <!-- -->
        <parent link="servo6_link"/>
        <child link="brat6_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="brat6_link">
        <visual>
            <origin xyz = "0.1 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/Brat_p_6.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <!-- Capace brate1 -->
    <joint name="capacbrat1_to_brat1" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat1_link"/>
        <child link="capacbrat1_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat1_link">
        <visual>
            <origin xyz = "0.1 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_1.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>


    <joint name="capacbrat2_to_brat2" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat2_link"/>
        <child link="capacbrat2_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat2_link">
        <visual>
            <origin xyz = "-0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_2.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="capacbrat3_to_brat3" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat3_link"/>
        <child link="capacbrat3_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat3_link">
        <visual>
            <origin xyz = "0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_3.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="capacbrat4_to_brat4" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat4_link"/>
        <child link="capacbrat4_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat4_link">
        <visual>
            <origin xyz = "-0.095 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_4.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="capacbrat5_to_brat5" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat5_link"/>
        <child link="capacbrat5_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat5_link">
        <visual>
            <origin xyz = "-0.1 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_5.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

    <joint name="capacbrat6_to_brat6" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>    <!-- -->
        <parent link="brat6_link"/>
        <child link="capacbrat6_link"/>
        <axis xyz="0 0 1"/>
        <limit lower="0" upper="1.57" effort="1.0" velocity="1.0"/>
    </joint>
    <link name="capacbrat6_link">
        <visual>
            <origin xyz = "0.1 -0.755 -0.325" rpy = "0 0 3.14159"/>
            <geometry>
                <mesh filename="package://urdfbot_description/meshes/capacBrat_p_6.stl" scale="0.01 0.01 0.01"/>
            </geometry>
        </visual>
    </link>

</robot>
