<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">

    <xacro:arg name="robot_type" default="a1"/>
    <xacro:include filename="$(find quadruped_gazebo)/urdf/$(arg robot_type)/const.xacro"/>

    <xacro:include filename="$(find quadruped_gazebo)/urdf/common/materials.xacro"/>
    <xacro:include filename="$(find quadruped_gazebo)/urdf/common/leg.xacro"/>
    <xacro:include filename="$(find quadruped_gazebo)/urdf/common/imu.xacro"/>
    <xacro:include filename="$(find quadruped_gazebo)/urdf/common/ros2_control.xacro"/>

    <xacro:property name="mesh_path" value="file://$(find quadruped_gazebo)/meshes/$(arg robot_type)"/>

    <link name="base">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
        </visual>
    </link>

    <joint name="floating_base" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <parent link="base"/>
        <child link="trunk"/>
    </joint>

    <link name="trunk">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="${mesh_path}/trunk.dae" scale="1 1 1"/>
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
            <mass value="${trunk_mass}"/>
            <inertia
                    ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
                    iyy="${trunk_iyy}" iyz="${trunk_iyz}"
                    izz="${trunk_izz}"/>
        </inertial>
    </link>

    <xacro:IMU connected_to="trunk" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>


    <xacro:leg prefix="LF" mesh_path="${mesh_path}" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg prefix="LH" mesh_path="${mesh_path}" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg prefix="RF" mesh_path="${mesh_path}" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
    </xacro:leg>

    <xacro:leg prefix="RH" mesh_path="${mesh_path}" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
    </xacro:leg>

</robot>
