<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
  <xacro:include filename="$(find fanuc_resources)/urdf/common_materials.xacro"/>

  <!-- single argument macro. For custom configurations, instantiate the base
       macro in a new top-level xacro. DO NOT edit this file.
   -->
  <xacro:macro name="fanuc_m900ib700" params="prefix">
    <xacro:fanuc_m900ib700_base prefix="${prefix}" has_plinks="true" has_balancers="true" has_fl_pockets="false" />
  </xacro:macro>

  <!-- base macro -->
  <xacro:macro name="fanuc_m900ib700_base" params="prefix has_plinks has_balancers has_fl_pockets">
    <!-- links: main serial chain -->
    <link name="${prefix}base_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/base_link.stl"/>
        </geometry>
        <xacro:material_fanuc_grey />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/base_link.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_1">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_1.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_1.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_2">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_2.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_2.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_3">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_3.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_3.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_4">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_4.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_4.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_5">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_5.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_5.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}link_6">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/link_6.stl"/>
        </geometry>
        <xacro:material_fanuc_black />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/link_6.stl"/>
        </geometry>
      </collision>
    </link>

    <!-- joints: main serial chain -->
    <joint name="${prefix}joint_1" type="revolute">
      <origin xyz="0 0 0.940" rpy="0 0 0"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}link_1"/>
      <axis xyz="0 0 1"/>
      <limit effort="0" lower="-3.1415" upper="3.1415" velocity="1.3962"/>
    </joint>
    <joint name="${prefix}joint_2" type="revolute">
      <origin xyz="0.410 0 0" rpy="0 0 0"/>
      <parent link="${prefix}link_1"/>
      <child link="${prefix}link_2"/>
      <axis xyz="0 1 0"/>
      <limit effort="0" lower="-1.1170" upper="1.5707" velocity="1.3962"/>
    </joint>
    <joint name="${prefix}joint_3" type="revolute">
      <origin xyz="0 0 1.120" rpy="0 0 0"/>
      <parent link="${prefix}link_2"/>
      <child link="${prefix}link_3"/>
      <axis xyz="0 -1 0"/>
      <limit effort="0" lower="-2.2689" upper="0.5235" velocity="1.3962"/>
    </joint>
    <joint name="${prefix}joint_4" type="revolute">
      <origin xyz="0 0 0.250" rpy="0 0 0"/>
      <parent link="${prefix}link_3"/>
      <child link="${prefix}link_4"/>
      <axis xyz="-1 0 0"/>
      <limit effort="0" lower="-6.2831" upper="6.2831" velocity="1.7453"/>
    </joint>
    <joint name="${prefix}joint_5" type="revolute">
      <origin xyz="1.285 0 0" rpy="0 0 0"/>
      <parent link="${prefix}link_4"/>
      <child link="${prefix}link_5"/>
      <axis xyz="0 -1 0"/>
      <limit effort="0" lower="-2.1293" upper="2.1293" velocity="1.7453"/>
    </joint>
    <joint name="${prefix}joint_6" type="revolute">
      <origin xyz="0.300 0 0" rpy="0 0 0"/>
      <parent link="${prefix}link_5"/>
      <child link="${prefix}link_6"/>
      <axis xyz="-1 0 0"/>
      <limit effort="0" lower="-6.2831" upper="6.2831" velocity="2.7925"/>
    </joint>

    <!-- parallel bar linkage -->
    <xacro:if value="${has_plinks}">
    <link name="${prefix}plink_1">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/counterweight.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/counterweight.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}plink_2">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/bar.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/bar.stl"/>
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}pjoint_1" type="continuous">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="${prefix}link_2"/>
      <child link="${prefix}plink_1"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_3" multiplier="-1" offset="0" />
    </joint>
    <joint name="${prefix}pjoint_2" type="continuous">
      <origin xyz="-0.75 0.08 0" rpy="0 0 0"/>
      <parent link="${prefix}link_3"/>
      <child link="${prefix}plink_2"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_3" multiplier="1" offset="0" />
    </joint>
    </xacro:if>

    <!-- balancers -->
    <xacro:if value="${has_balancers}">
    <link name="${prefix}balancer_left">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/balancer.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/balancer.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}piston_left">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/piston.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/piston.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}balancer_right">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/balancer.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/balancer.stl"/>
        </geometry>
      </collision>
    </link>
    <link name="${prefix}piston_right">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/piston.stl"/>
        </geometry>
        <xacro:material_fanuc_yellow />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/piston.stl"/>
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}balancer_left_joint" type="continuous">
      <origin xyz="0 0 1.120" rpy="0 0 0"/>
      <parent link="${prefix}link_2"/>
      <child link="${prefix}balancer_left"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_2" multiplier="0.18" offset="0" />
    </joint>
    <joint name="${prefix}piston_left_joint" type="continuous">
      <origin xyz="0.410 0 0.235" rpy="0 0 0"/>
      <parent link="${prefix}link_1"/>
      <child link="${prefix}piston_left"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_2" multiplier="1.18" offset="0" />
    </joint>
    <joint name="${prefix}balancer_right_joint" type="continuous">
      <origin xyz="0 0 1.120" rpy="0 0 0"/>
      <parent link="${prefix}link_2"/>
      <child link="${prefix}balancer_right"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_2" multiplier="0.18" offset="0" />
    </joint>
    <joint name="${prefix}piston_right_joint" type="continuous">
      <origin xyz="0.410 0 0.235" rpy="0 0 0"/>
      <parent link="${prefix}link_1"/>
      <child link="${prefix}piston_right"/>
      <axis xyz="0 1 0"/>
      <mimic joint="${prefix}joint_2" multiplier="1.18" offset="0" />
    </joint>
    </xacro:if>

    <!-- forklift pockets -->
    <xacro:if value="${has_fl_pockets}">
    <link name="${prefix}forklift_pockets">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/visual/forklift_pockets.stl"/>
        </geometry>
        <xacro:material_fanuc_black />
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://fanuc_m900ib_support/meshes/m900ib700/collision/forklift_pockets.stl"/>
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}forklift_pockets_joint" type="fixed">
      <origin xyz="0 0 ${-(0.940 - 0.445)}" rpy="0 0 0"/>
      <parent link="${prefix}link_1"/>
      <child link="${prefix}forklift_pockets"/>
    </joint>
    </xacro:if>

    <!-- ROS-Industrial 'base' frame: base_link to Fanuc World Coordinates transform -->
    <link name="${prefix}base" />
    <joint name="${prefix}base_link-base" type="fixed">
      <origin xyz="0 0 0.940" rpy="0 0 0"/>
      <parent link="${prefix}base_link"/>
      <child link="${prefix}base"/>
    </joint>

    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
    <link name="${prefix}flange" />
    <joint name="${prefix}joint_6-flange" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <parent link="${prefix}link_6" />
      <child link="${prefix}flange" />
    </joint>

    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
    <link name="${prefix}tool0" />
    <joint name="${prefix}link_6-tool0" type="fixed">
      <origin xyz="0 0 0" rpy="${pi} ${-pi/2.0} 0" />
      <parent link="${prefix}link_6" />
      <child link="${prefix}tool0" />
    </joint>
  </xacro:macro>
</robot>
