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

    <!--................................ INERTIA MATRICES .............................. -->

    <xacro:macro name="cylinder_inertia" params="m r h">
        <inertia ixx="${m * (3 * r * r + h * h) / 12}" ixy="0" ixz="0" iyy="${m * (3 * r * r + h * h) / 12}" iyz="0" izz="${m * r * r / 2}" />
    </xacro:macro>

    <xacro:macro name="box_inertia" params="m x y z">
        <inertia ixx="${m * (y * y + z * z) / 12}" ixy="0" ixz="0" iyy="${m * (x * x + z * z) / 12}" iyz="0" izz="${m * (y * y + x * x) / 12}" />
    </xacro:macro>

    <xacro:macro name="sphere_inertia" params="m r">
        <inertia ixx="${2 * m * r * r / 5}" ixy="0" ixz="0" iyy="${2 * m * r * r / 5}" iyz="0" izz="${2 * m * r * r / 5}" />
    </xacro:macro>

    <!-- ............................... TROLLEY WHEEL DEFINITION .............................. -->

    <xacro:macro name="trolley_wheel" params="cardinality dexterity origin_x origin_y origin_z">
        <link name="${cardinality}_${dexterity}_wheel">
            <collision>
                <origin xyz="0 0 0" rpy="0 ${pi/2} ${pi/2}" />
                <geometry>
                    <sphere radius="${trolley_wheel_radius}"/>
                </geometry>
            </collision>

            <inertial>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <mass value="${trolley_wheel_mass}"/>
                <xacro:sphere_inertia m="${trolley_wheel_mass}" r="${trolley_wheel_radius}"/>
            </inertial>

            <mu>0.0</mu>
        </link>

        <joint name="${cardinality}_${dexterity}_wheel_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${cardinality}_${dexterity}_wheel"/>
            <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" rpy="0 0 0" />
        </joint>

        <gazebo reference="${cardinality}_${dexterity}_wheel">
            <material>Gazebo/Black</material>
        </gazebo>
    </xacro:macro>

    <xacro:macro name="traction_wheel" params="cardinality dexterity origin_x origin_y origin_z">
        <link name="${cardinality}_${dexterity}_wheel">
            <collision>
                <origin xyz="0 0 0" rpy="0 ${pi/2} ${pi/2}" />
                <geometry>
                    <cylinder length="${traction_wheel_width}" radius="${traction_wheel_radius}"/>
                </geometry>
                <self_collide>false</self_collide>
            </collision>

            <inertial>
                <origin xyz="0 0 0" rpy="0 ${pi/2} ${pi/2}" />
                <mass value="${traction_wheel_mass}"/>
                <xacro:cylinder_inertia m="${traction_wheel_mass}" r="${traction_wheel_radius}" h="${traction_wheel_width}"/>
            </inertial>

            <mu>5.0</mu>
        </link>

        <joint name="${cardinality}_${dexterity}_wheel_joint" type="continuous">
            <parent link="base_link"/>
            <child link="${cardinality}_${dexterity}_wheel"/>
            <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" rpy="0 0 0" />
        </joint>

        <gazebo reference="${cardinality}_${dexterity}_wheel">
            <material>Gazebo/Black</material>
        </gazebo>
    </xacro:macro>

</robot>