<?xml version="1.0"?>

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

    <!-- B1 macro: describes Unitree B1 quadruped robot -->
    <xacro:macro name="b1">

        <xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
        <xacro:include filename="$(find b1_description)/xacro/materials.xacro"/>
        <xacro:include filename="$(find b1_description)/xacro/leg.xacro"/>

        <!-- Rotor related joint and link is only for demonstrate location. -->
        <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->

        <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="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
                </geometry>
                <material name="orange"/>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
                </geometry>
            </collision>
            <!-- <collision>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
                </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>

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

        <link name="imu_link">
            <inertial>
                <mass value="0.001"/>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
            </inertial>
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
                <material name="red"/>
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <box size=".001 .001 .001"/>
                </geometry>
            </collision>
        </link>

        <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
        <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
        <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
        <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />

    </xacro:macro>

</robot>
