<?xml version="1.0"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from aubo_i5L.urdf                  | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->

<robot name="aubo_i5" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find aubo_description)/urdf/aubo.transmission.xacro" />
  <xacro:include filename="$(find aubo_description)/urdf/materials.xacro" />
  <xacro:include filename="$(find aubo_description)/urdf/ros2_control.xacro" />
  <xacro:include filename="$(find dh_ag95_description)/urdf/dh_ag95_macro.urdf.xacro" />
  <xacro:include filename="$(find dh_ag145_description)/urdf/dh_ag145_macro.urdf.xacro" />
  <xacro:include filename="$(find aubo_description)/urdf/sensors.xacro" />
  <xacro:include filename="$(find simple_gz_gripper)/urdf/simple_gripper_macro.xacro"/>

  <xacro:property name="hardware" value="$(arg hardware)"/>
  <xacro:property name="robot_ip" value="$(arg robot_ip)"/>
  <xacro:property name="with_gripper" value="$(arg with_gripper)"/>
  <xacro:property name="simulation_controllers" value="$(arg simulation_controllers)"/>


  <link name="base_link">
    <inertial>
      <origin xyz="-1.4795E-13 0.0015384 0.020951" rpy="0 0 0" />
      <mass value="0.83419" />
      <inertia ixx="0.0014414" ixy="7.8809E-15" ixz="8.5328E-16" iyy="0.0013542" iyz="-1.4364E-05"
        izz="0.0024659" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/base_link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/base_link.STL" />
      </geometry>
    </collision>
  </link>

  <link name="shoulder_Link">
    <inertial>
      <origin xyz="3.2508868974735E-07 0.00534955349296065 -0.00883689325611056" rpy="0 0 0" />
      <mass value="1.57658348693929" />
      <inertia ixx="0.0040640448663128" ixy="0" ixz="0" iyy="0.00392863238466817"
        iyz="-0.000160151642851425" izz="0.0030869857349184" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/shoulder_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/shoulder_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="shoulder_joint" type="revolute">
    <origin xyz="0 0 0.122" rpy="0 0 3.1416" />
    <parent link="base_link" />
    <child link="shoulder_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="150" velocity="3.1416" />
  </joint>

  <link name="upperArm_Link">
    <inertial>
      <origin xyz="0.203996646979614 2.01304585036544E-10 0.0127641545395984" rpy="0 0 0" />
      <mass value="4.04175782265494" />
      <inertia ixx="0.00965399211106204" ixy="0" ixz="0" iyy="0.144993869035655" iyz="0"
        izz="0.142607184038966" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/upperArm_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/upperArm_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="upperArm_joint" type="revolute">
    <origin xyz="0 0.1215 0" rpy="-1.5708 -1.5708 0" />
    <parent link="shoulder_Link" />
    <child link="upperArm_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="150" velocity="3.1416" />
  </joint>

  <link name="foreArm_Link">
    <inertial>
      <origin xyz="0.188922115560337 6.78882434739072E-07 0.0981026740461557" rpy="0 0 0" />
      <mass value="2.27145669098343" />
      <inertia ixx="0.00214322284946289" ixy="0" ixz="-0.00073120631553383" iyy="0.0443926090878205"
        iyz="0" izz="0.0441273797128365" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/foreArm_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/foreArm_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="foreArm_joint" type="revolute">
    <origin xyz="0.408 0 0" rpy="-3.1416 -5.1632E-18 -5.459E-16" />
    <parent link="upperArm_Link" />
    <child link="foreArm_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="150" velocity="3.1416" />
  </joint>

  <link name="wrist1_Link">
    <inertial>
      <origin xyz="7.54205137428592E-07 0.0062481254331257 -0.00392367464072373" rpy="0 0 0" />
      <mass value="0.500477539188764" />
      <inertia ixx="0.00071194605962081" ixy="0" ixz="0" iyy="0.00040588242872958"
        iyz="-2.30808694377512E-05" izz="0.000685574004861334" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist1_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist1_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist1_joint" type="revolute">
    <origin xyz="0.376 0 0" rpy="3.1416 -1.8323E-15 1.5708" />
    <parent link="foreArm_Link" />
    <child link="wrist1_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="28" velocity="3.1416" />
  </joint>

  <link name="wrist2_Link">
    <inertial>
      <origin xyz="-7.54207620578635E-07 -0.00624812542617262 -0.00392367464115684" rpy="0 0 0" />
      <mass value="0.500477539245988" />
      <inertia ixx="0.00071194605981829" ixy="0" ixz="0" iyy="0.000405882428755442"
        iyz="2.30808694515886E-05" izz="0.000685574005112107" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist2_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist2_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist2_joint" type="revolute">
    <origin xyz="0 0.1025 0" rpy="-1.5708 -1.8709E-15 -1.6653E-16" />
    <parent link="wrist1_Link" />
    <child link="wrist2_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="28" velocity="3.1416" />
  </joint>

  <link name="wrist3_Link">
    <inertial>
      <origin xyz="3.92048778449938E-10 0.000175788057281467 -0.0213294490706684" rpy="0 0 0" />
      <mass value="0.158309554874285" />
      <inertia ixx="7.31376196034769E-05" ixy="0" ixz="0" iyy="7.19528188876563E-05" iyz="0"
        izz="0.000108772439051422" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist3_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist3_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist3_joint" type="revolute">
    <origin xyz="0 -0.094 0" rpy="1.5708 0 1.7907E-15" />
    <parent link="wrist2_Link" />
    <child link="wrist3_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-6.28" upper="6.28" effort="28" velocity="3.1416" />
  </joint>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link="base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

    <!-- <xacro:dh_ag145_gripper prefix="" parent="wrist3_Link"> -->
    <!--   <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> -->
    <!-- </xacro:dh_ag145_gripper> -->
  <xacro:simple_parallel_gripper prefix="" parent="wrist3_Link">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 1.57" />
  </xacro:simple_parallel_gripper>


  <xacro:rgbd parent="wrist3_Link" tf_prefix="inner_">
    <origin xyz="0.0 0.05 0.0" rpy="0.0 -1.47 -1.57" />
  </xacro:rgbd>

  <xacro:rgbd parent="base_link" tf_prefix="outer_">
    <origin xyz="0.62179261445999146 -0.83430188894271851 0.85204130411148071" rpy="0.00 0.608 1.934" />
  </xacro:rgbd>

  <xacro:aubo_ros2_control hardware="${hardware}" robot_ip="${robot_ip}" />
  <xacro:if value="${hardware=='gazebo'}">
    <gazebo>
      <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
        <parameters>${simulation_controllers}</parameters>
        <!-- <ros> -->
        <!--   <namespace>${namespace}</namespace> -->
        <!-- </ros> -->
      </plugin>
    </gazebo>
  </xacro:if>

</robot>
