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

  <material name="orange">
    <color rgba="1.0 0.5 0.0 1"/>
  </material>
  <material name="black">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <material name="grey">
    <color rgba="0.6 0.6 0.6 1"/>
  </material>

  <xacro:macro name="rev_joint" params="name type axis_xyz origin_rpy origin_xyz parent child effort velocity damping friction upper lower">
    <joint name="${name}" type="${type}">
      <axis xyz="${axis_xyz}" />
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <parent link="${parent}" />
      <child link="${child}" />
      <limit effort="${effort}" velocity="${velocity}" lower="${lower}" upper="${upper}"/>
      <joint_properties damping="${damping}" friction="${friction}" />
    </joint>
  </xacro:macro>

  <xacro:macro name="frame_joint" params="name type origin_rpy origin_xyz parent child">
    <joint name="${name}" type="${type}">
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <parent link="${parent}" />
      <child link="${child}" />
    </joint>
  </xacro:macro>

  <xacro:macro name="mesh_link" params="name origin_xyz origin_rpy meshfile meshscale c_xyz matname mass i_xyz i_rpy length width height">
    <link name="${name}">
      <visual>
        <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
        <geometry>
          <mesh filename="${meshfile}" scale="${meshscale}"/>
        </geometry>
        <material name="${matname}"/>
      </visual>

      <collision>
        <origin rpy="${origin_rpy}" xyz="${c_xyz}" />
        <geometry>
          <box size="${length} ${width} ${height}" />
        </geometry>
      </collision>

      <inertial>
        <mass value="${mass}" />
        <origin rpy="${i_rpy}" xyz="${i_xyz}" /> <!-- Inertial Axes -->
        <inertia ixx="${mass * (width * width + height * height)/12.0}" ixy="${0}" ixz="${0}" iyx="${0}" iyy="${mass * (length * length + height * height)/12.0}" iyz="${0}" izx="${0}" izy="${0}" izz="${mass * (length * length + width * width)/12.0}" />
      </inertial>
    </link>
    <gazebo reference="${name}">
        <xacro:if value="${matname == 'black'}">
          <material>Gazebo/Black</material>
        </xacro:if>
        <xacro:if value="${matname == 'orange'}">
          <material>Gazebo/Orange</material>
        </xacro:if>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="mesh_link_sphere" params="name origin_xyz origin_rpy meshfile meshscale c_xyz matname mass i_xyz i_rpy length width height radius">
    <link name="${name}">
      <visual>
        <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
        <geometry>
          <mesh filename="${meshfile}" scale="${meshscale}"/>
        </geometry>
        <material name="${matname}"/>
      </visual>

      <collision>
        <origin rpy="${origin_rpy}" xyz="${c_xyz}" />
        <geometry>
          <mesh filename="${meshfile}" scale="${meshscale}"/>
        </geometry>
        <contact_coefficients mu="10.1"/>
      </collision>

      <inertial>
        <mass value="${mass}" />
        <origin rpy="${i_rpy}" xyz="${i_xyz}" /> <!-- Inertial Axes -->
        <inertia ixx="${mass * (width * width + height * height)/12.0}" ixy="${0}" ixz="${0}" iyx="${0}" iyy="${mass * (length * length + height * height)/12.0}" iyz="${0}" izx="${0}" izy="${0}" izz="${mass * (length * length + width * width)/12.0}" />
      </inertial>
    </link>

    <gazebo reference="${name}">
      <kp>1000000.0</kp>
      <kd>1.0</kd>
      <mu1>0.8</mu1>
      <mu2>0.8</mu2>
      <maxVel>0.0</maxVel>
      <minDepth>0.001</minDepth>
      <material>Gazebo/Orange</material>
    </gazebo>
  </xacro:macro>


  <!-- Left Leg: prefix is front or rear -->
<!--   <xacro:macro name="LeftLeg" params="prefix reflect">
    <link name="${prefix}_Left_Leg">
        <visual>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <joint name="base_to_${prefix}_leg" type="fixed">
        <parent link="base_link"/>
        <child link="${prefix}_leg"/>
        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
    </joint>
</xacro:macro> -->

</robot>