<?xml version="1.0"?>

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

    <xacro:arg name="DEBUG" default="false"/>

    <xacro:include filename="$(find go2_description)/xacro/const.xacro"/>
    <xacro:include filename="$(find go2_description)/xacro/materials.xacro"/>
    <xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
    <!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
    <xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
    <!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->

    <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->

    <!-- 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. -->

    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
    <xacro:if value="$(arg DEBUG)">
        <link name="world"/>
        <joint name="base_static_joint" type="fixed">
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <parent link="world"/>
            <child link="base_link"/>
        </joint>
    </xacro:if> 

    <link name="base_link">
        <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_link"/>
        <child link="trunk"/>
    </joint>

    <link name="trunk">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="package://go2_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>
        <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.001697 -0.020900 -0.000007" xyz="-0.027353 -0.005499 0.039913"/>
    </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>

    <joint name="vicon_joint" type="fixed">
        <parent link="trunk"/>
        <child link="vicon_link"/>
        <origin rpy="-0.037935 -0.066371 -2.668897" xyz="0.022944 -0.000007 0.058458"/>
    </joint>

    <link name="vicon_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <!-- <material name="red"/> -->
        </visual>
    </link>


    <joint name="infra1_joint" type="fixed">
        <parent link="trunk"/>
        <child link="infra1_link"/>
        <origin rpy="-1.641669 -0.014332 -1.617155" xyz="0.321618 0.033305 0.081622"/>
    </joint>

    <link name="infra1_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <!-- <material name="red"/> -->
        </visual>
    </link>

    <joint name="infra2_joint" type="fixed">
        <parent link="trunk"/>
        <child link="infra2_link"/>
        <origin rpy="-1.640637 -0.014093 -1.611177" xyz="0.317969 -0.061919 0.082773"/>
    </joint>

    <link name="infra2_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <!-- <material name="red"/> -->
        </visual>
    </link>

    <joint name="color_joint" type="fixed">
        <parent link="trunk"/>
        <child link="color_link"/>
        <origin rpy="-1.640491 -0.011125 -1.610364" xyz="0.319415 -0.025790 0.082311"/>
    </joint>

    <link name="color_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <!-- <material name="red"/> -->
        </visual>
    </link>

    <!-- <link name="utlidar">
        <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="0"/>
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>
    <joint name="utlidar_joint" type="fixed">
        <origin rpy="0 2.8782 0" xyz="0.28945 0 -0.046825"/>
        <parent link="base_link"/>
        <child link="utlidar"/>
        <axis xyz="0 0 0"/>
    </joint> -->

    <link name="hesai">
        <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="0"/>
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>
    <joint name="hesai_joint" type="fixed">
        <origin rpy="0 0 1.57075" xyz="0.15 0 0.25"/>
        <parent link="base_link"/>
        <child link="hesai"/>
        <axis xyz="0 0 0"/>
    </joint>
    
    <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" />
</robot>
