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

  <property name="M_PI" value="3.14159"/>
  <property name="M_SCALE" value="0.001"/>
  <property name="MESH_SCALE" value="100"/>
  <property name="F10_HEIGHT" value="0.004"/>
  <property name="F4_HEIGHT" value="0.0525"/>
  <property name="F3_HEIGHT" value="0.009"/>
  <property name="AX12_HEIGHT" value="0.0385"/>
  <property name="AX12_WIDTH" value="0.038"/>
  <property name="AX12_DEPTH" value="0.048"/>
  <property name="F2_HEIGHT" value="0.0265"/>

  <macro name="bioloid_F10_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/F10.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.038 0.004"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="finger_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/finger.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.02645 0.0 -0.00655" rpy="0 0 0" />
        <geometry>
          <box size="0.0783 0.03801 0.0193"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="bioloid_F3_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/F3.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 -0.0045" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.038 0.009"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="dynamixel_AX12_fixed" params="parent name *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/ax12_box.${MESH_EXT}" scale="${MESH_SCALE} ${MESH_SCALE} ${MESH_SCALE}"/>
        </geometry>
        <material name="black"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 -0.01241" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.038 0.04762"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="bioloid_F3_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}_joint" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 0 -1"/>
      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/F3.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 -0.0045" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.038 0.009"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="bioloid_F2_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}_joint" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 1 0"/>
      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/F2.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 0.00775" rpy="0 0 0" />
        <geometry>
          <box size="0.025 0.0485 0.0375"/>
        </geometry>
      </collision>
    </link>
  </macro>

  <macro name="bioloid_F4_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}_joint" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 1 0"/>
      <limit effort="30" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.00001" />
        <origin xyz="0 0 0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0"
          iyy="1.0" iyz="0.0"
          izz="1.0" />
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://rbx1_description/meshes/F4.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz="0.0 0.0 0.0215" rpy="0 0 0" />
        <geometry>
          <box size="0.028 0.0485 0.065"/>
        </geometry>
      </collision>
    </link>
  </macro>

</robot>
