<?xml version="1.0"?>

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

    <xacro:include filename="$(find quadruped_gazebo)/urdf/common/transmission.xacro"/>

    <xacro:macro name="leg" params="prefix mesh_path mirror mirror_dae front_hind front_hind_dae *origin">

        <joint name="${prefix}_HAA" type="revolute">
            <xacro:insert_block name="origin"/>
            <parent link="trunk"/>
            <child link="${prefix}_hip"/>
            <axis xyz="1 0 0"/>
            <dynamics damping="${damping}" friction="${friction}"/>
            <xacro:if value="${(mirror_dae == True)}">
                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
                       upper="${hip_position_max}"/>
            </xacro:if>
            <xacro:if value="${(mirror_dae == False)}">
                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
                       upper="${-hip_position_min}"/>
            </xacro:if>
        </joint>

        <link name="${prefix}_hip">
            <visual>
                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
                    <origin rpy="0 0 0" xyz="0 0 0"/>
                </xacro:if>
                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
                    <origin rpy="${pi} 0 0" xyz="0 0 0"/>
                </xacro:if>
                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
                    <origin rpy="0 ${pi} 0" xyz="0 0 0"/>
                </xacro:if>
                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
                    <origin rpy="${pi} ${pi} 0" xyz="0 0 0"/>
                </xacro:if>
                <geometry>
                    <mesh filename="${mesh_path}/hip.dae"
                          scale="1 1 1"/>
                </geometry>
<!--                <material name="orange"/>-->
            </visual>
            <collision>
                <origin rpy="${pi/2.0} 0 0" xyz="0 0 0"/>
                <geometry>
                    <cylinder length="${hip_length}" radius="${hip_radius}"/>
                </geometry>
            </collision>
            <inertial>
                <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
                <mass value="${hip_mass}"/>
                <inertia
                        ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
                        iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
                        izz="${hip_izz}"/>
            </inertial>
        </link>

        <joint name="${prefix}_HFE" type="revolute">
            <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
            <parent link="${prefix}_hip"/>
            <child link="${prefix}_thigh"/>
            <axis xyz="0 1 0"/>
            <dynamics damping="${damping}" friction="${friction}"/>
            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
                   upper="${thigh_position_max}"/>
        </joint>

        <link name="${prefix}_thigh">
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <xacro:if value="${mirror_dae == True}">
                        <mesh filename="${mesh_path}/thigh.dae"
                              scale="1 1 1"/>
                    </xacro:if>
                    <xacro:if value="${mirror_dae == False}">
                        <mesh filename="${mesh_path}/thigh_mirror.dae"
                              scale="1 1 1"/>
                    </xacro:if>
                </geometry>
<!--                <material name="orange"/>-->
            </visual>
            <collision>
                <origin rpy="0 ${pi/2.0+0.1} 0" xyz="${thigh_x_offset} 0 ${-thigh_length/2.0}"/>
                <geometry>
                    <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
                </geometry>
            </collision>
            <collision>
                <origin rpy="${pi/2.0} 0 0" xyz="0 ${mirror*thigh_shoulder_y_offset} 0"/>
                <geometry>
                    <cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
                </geometry>
            </collision>
            <inertial>
                <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
                <mass value="${thigh_mass}"/>
                <inertia
                        ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
                        iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
                        izz="${thigh_izz}"/>
            </inertial>
        </link>

        <joint name="${prefix}_KFE" type="revolute">
            <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
            <parent link="${prefix}_thigh"/>
            <child link="${prefix}_calf"/>
            <axis xyz="0 1 0"/>
            <dynamics damping="${damping}" friction="${friction}"/>
            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
        </joint>

        <link name="${prefix}_calf">
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <mesh filename="${mesh_path}/calf.dae"
                          scale="1 1 1"/>
                </geometry>
<!--                <material name="orange"/>-->
            </visual>
            <collision>
                <origin rpy="0 ${pi/2.0} 0" xyz="${calf_x_offset} 0 ${-calf_length/2.0}"/>
                <geometry>
                    <box size="${calf_length} ${calf_width} ${calf_height}"/>
                </geometry>
            </collision>
            <inertial>
                <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
                <mass value="${calf_mass}"/>
                <inertia
                        ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
                        iyy="${calf_iyy}" iyz="${calf_iyz}"
                        izz="${calf_izz}"/>
            </inertial>
        </link>

        <joint name="${prefix}_foot_fixed" type="fixed" dont_collapse="true">
            <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
            <parent link="${prefix}_calf"/>
            <child link="${prefix}_FOOT"/>
        </joint>

        <link name="${prefix}_FOOT">
            <visual>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <sphere radius="${foot_radius-0.01}"/>
                </geometry>
<!--                <material name="orange"/>-->
            </visual>
            <collision>
                <origin rpy="0 0 0" xyz="0 0 0"/>
                <geometry>
                    <sphere radius="${foot_radius}"/>
                </geometry>
            </collision>
            <inertial>
                <mass value="${foot_mass}"/>
                <inertia
                        ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
                        iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
                        izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
            </inertial>
        </link>

        <gazebo reference="${prefix}_foot_fixed">
            <disableFixedJointLumping>true</disableFixedJointLumping>
            <sensor name="${prefix}_foot_end" type="force_torque">
                <update_rate>30</update_rate>
            </sensor>
        </gazebo>

        <gazebo reference="${prefix}_hip">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <material>
                <ambient>.175 .175 .175  1.0</ambient>
                <diffuse>.175 .175 .175  1.0</diffuse>
                <specular>.175 .175 .175 1.000000 1.500000</specular>
            </material>
        </gazebo>
        <gazebo reference="${prefix}_thigh">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <self_collide>0</self_collide>
            <material>
                <ambient>.175 .175 .175  1.0</ambient>
                <diffuse>.175 .175 .175  1.0</diffuse>
                <specular>.175 .175 .175 1.000000 1.500000</specular>
            </material>
            <kp value="1000000.0"/>
            <kd value="100.0"/>
        </gazebo>

        <gazebo reference="${prefix}_calf">
            <mu1>0.6</mu1>
            <mu2>0.6</mu2>
            <self_collide>1</self_collide>
            <material>
                <ambient>.175 .175 .175  1.0</ambient>
                <diffuse>.175 .175 .175  1.0</diffuse>
                <specular>.175 .175 .175 1.000000 1.500000</specular>
            </material>
        </gazebo>

        <gazebo reference="${prefix}_FOOT">
            <mu1>0.6</mu1>
            <mu2>0.6</mu2>
            <self_collide>1</self_collide>
            <material>
                <ambient>.175 .175 .175  1.0</ambient>
                <diffuse>.175 .175 .175  1.0</diffuse>
                <specular>.175 .175 .175 1.000000 1.500000</specular>
            </material>
            <kp value="1000000.0"/>
            <kd value="100.0"/>

            <sensor name='${prefix}_Foot_Contact_Sensor' type='contact'>
                <contact>
                    <collision>${prefix}_FOOT_collision</collision>
                </contact>
            </sensor>
        </gazebo>

        <xacro:leg_transmission name="${prefix}"/>
    </xacro:macro>
</robot>
