<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="create_frame_plate" params="z_offset">
        <visual>
            <origin xyz="0 0 ${z_offset}"/>
            <geometry>
              <box size="0.100 0.180 0.003"/>        
            </geometry>
            <material name="black"/>
        </visual>
    </xacro:macro>
  
    <xacro:macro name="create_coxa_joint_mount" params="xy_offset">
      <visual>
          <origin xyz="${xy_offset} -0.0325"/>
          <geometry>
              <cylinder radius="0.0125" length="0.006"/>
          </geometry>
          <material name="black"/>
      </visual>
      <visual>
          <origin xyz="${xy_offset} 0.0325"/>
          <geometry>
              <cylinder radius="0.0125" length="0.006"/>
          </geometry>
          <material name="black"/>
      </visual>
    </xacro:macro>
    
    <xacro:macro name="create_coxa" params="name side xy_offset">
        <link name="${name}_link">
            <!-- coxa bounding box -->
            <visual>
                <geometry>
                    <box size="0.045 0.045 0.045"/>
                </geometry>
		<xacro:if value="${side == 'l'}"><origin xyz="-0.015 -0.005 0"/></xacro:if>
		<xacro:if value="${side == 'r'}"><origin xyz=" 0.015 -0.005 0"/></xacro:if>
                <material name="blue"/>                                
            </visual>
            <!-- coxa rotation axis -->
            <visual>
                <geometry>
                    <cylinder radius="0.005" length="0.065"/>
                </geometry>
                <material name="blue"/>
            </visual>
            <!-- femur rotation axis -->
            <visual>
		<xacro:if value="${side == 'l'}"><origin xyz="-0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
		<xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
                <geometry>
                      <cylinder radius="0.005" length="0.065"/>
                </geometry>
                <material name="blue"/>
            </visual>                       
        </link>
        <joint name="${name}_joint" type="revolute">
            <parent link="base_link"/>
            <child  link="${name}_link"/>
            <origin xyz="${xy_offset} 0"/>
            <axis   xyz="0 0 1"/>
            <limit effort="30" velocity="100" lower="-1" upper="1"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="create_femur" params="name parent_name side">
        <link name="${name}_link">
            <visual>
                <xacro:if value="${side == 'l'}"><origin xyz="-0.040 0 0"/></xacro:if>
                <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 0 0"/></xacro:if>
                <geometry>
                    <box size="0.100 0.003 0.010"/>
                </geometry>
                <material name="green"/>
            </visual>
<!--
            <visual>
                <xacro:if value="${side == 'l'}"><origin xyz="-0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
                <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
                <geometry>
                    <cylinder radius="0.0125" length="0.006"/>
                </geometry>
                <material name="green"/>
            </visual>
-->           
        </link>
        <joint name="${name}_joint" type="revolute">
            <parent link="${parent_name}_link"/>
            <child  link="${name}_link"/>
            <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
            <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
            <axis xyz="0 -1 0"/>
            <limit effort="30" velocity="100" lower="-1" upper="1"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="create_tibia" params="name parent_name side">
        <link name="${name}_link">
            <!-- servo box -->
            <visual>
                <origin xyz="0 -0.005 0.010"/>
                <geometry>
                    <box size="0.020 0.035 0.040"/> 
                </geometry>
                <material name="red"/>
            </visual>
            <!-- tibia top -->
            <visual>
                <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 0.0025"/></xacro:if>
                <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 0.0025"/></xacro:if>
                <geometry>
                    <box size="0.040 0.003 0.075"/> 
                </geometry>
                <material name="red"/>
            </visual>
	    <!-- tibia middle -->
            <visual>
                <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 -0.055"/></xacro:if>
                <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 -0.055"/></xacro:if>
                <origin xyz="0 0 -0.055"/>
                <geometry>
                    <box size="0.030 0.003 0.050"/> 
                </geometry>
                <material name="red"/>
            </visual>
	    <!-- tibia bottom -->
            <visual>
                <origin xyz=" 0.0 0 -0.105"/>
                <origin xyz="0 0 -0.105"/>
                <geometry>
                    <box size="0.010 0.003 0.050"/> 
                </geometry>
                <material name="red"/>
            </visual>
            <!-- tibia rotation axis -->
            <visual>
		<origin xyz="0 0 0" rpy="-1.57 0 0"/>
                <geometry>
                      <cylinder radius="0.005" length="0.065"/>
                </geometry>
                <material name="red"/>
            </visual>                       

        </link>
        <joint name="${name}_joint" type="revolute">
            <parent link="${parent_name}_link"/>
            <child  link="${name}_link"/>
            <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
            <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
            <axis xyz="0 1 0"/>
            <limit effort="30" velocity="100" lower="-1" upper="1"/>
        </joint>
    </xacro:macro>

    <xacro:macro name="create_end" params="name side parent_name">
        <link name="${name}_link">
            <visual>
                <geometry>
                    <sphere radius="0.004"/>
                </geometry>
                <material name="cyan"/>
            </visual>
        </link>
        <joint name="${name}_joint" type="fixed">
            <parent link="${parent_name}_link"/>
            <child  link="${name}_link"/>
            <origin xyz="0 0 -0.130"/>
        </joint>
    </xacro:macro>
    
    <xacro:macro name="create_leg" params="number side xy_offset">
        <xacro:create_coxa  name="c${number}" side="${side}" xy_offset="${xy_offset}"/>
        <xacro:create_femur name="f${number}" side="${side}" parent_name="c${number}"/>
        <xacro:create_tibia name="t${number}" side="${side}" parent_name="f${number}"/>
        <xacro:create_end   name="e${number}" side="${side}" parent_name="t${number}"/>
    </xacro:macro>
    
</robot>
