<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/mike/workspaces/catkin_ws/src/calibration/atom/atom_examples/spjbot/spjbot_description/urdf/scara.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="scara" xmlns:xacro="http://ros.org/wiki/xacro">


    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- Tables properties-->
    <xacro:property name="table_height" value="0.795"/>
    <xacro:property name="table_width" value=".9"/>
    <xacro:property name="table_lenght" value="1.9"/>
    <xacro:property name="table_tabletop_thickness" value="0.015"/>
    <xacro:property name="table_leg_radius" value="0.02"/>

    <!-- ####################################### -->
    <!-- Include files-->
    <!-- ####################################### -->

    <!-- Scene objets-->
    <xacro:include filename="$(find spjbot_description)/urdf/properties.xacro"/>
    <xacro:include filename="$(find spjbot_description)/urdf/table.macro.xacro"/>

    <!-- Sensors-->
    <xacro:include filename="$(find spjbot_description)/urdf/rgb_camera.urdf.xacro" />

    <!-- Manipulator-->
    <xacro:include filename="$(find ur_gazebo)/urdf/ur_macro.xacro"/>

    <!-- <xacro:include filename="$(find spjbot_description)/urdf/scara.urdf.xacro" /> -->

    <!-- ####################################### -->
    <!-- Declare links and joints -->
    <!-- ####################################### -->
    <link name="world"/>

    <!-- Place table 1 in the scene-->
    <xacro:table_macro name="table" parent="world" width="${table_width}"
                       lenght="${table_lenght}" height="${table_height}"
                       tabletop_thickness="${table_tabletop_thickness}" leg_radius="${table_leg_radius}">
        <origin xyz="-1 0 0" rpy="0 0 ${M_PI/2}" />
    </xacro:table_macro>

    <!-- Place table_small on top of table-->
    <xacro:table_macro name="table_small" parent="tabletop_table" width="0.6"
                       lenght="0.4" height="0.35"
                       tabletop_thickness="${table_tabletop_thickness}" leg_radius="${table_leg_radius}">
        <origin xyz="0 0.7 0.001" rpy="0 0 0" />
    </xacro:table_macro>


    <link name="base_link" />

    <!--  Place robot on top of the table_small -->
    <joint name="tabletop_table_to_robot" type="fixed">
        <parent link="tabletop_table_small" />
        <child link = "base_link"/>
        <origin xyz="0.0 -0.1 0.0001" rpy="0.0 0.0 -1.57" />
    </joint>


    <!-- ####################################### -->
    <!-- Arguments and propperties-->
    <!-- ####################################### -->
    <!--Args are local, properties are global, so we copy the values of the arguments to the properties-->
    <!--Properties are global so they can be viewed from inside the included xacros-->
    <!--  args are used with $(arg arg_name), properties are used as ${property_name}-->
    <!-- Define materials -->
    <material name="yellow">
        <color rgba="0.88 0.69 0 1" />
    </material>
    <material name="dark_grey">
        <color rgba="0.4 0.4 0.4 1" />
    </material>
    <material name="black">
        <color rgba="0 0 0 1" />
    </material>
    <!-- ####################################### -->
    <!-- Include files-->
    <!-- ####################################### -->
    <!-- ####################################### -->
    <!-- Declare links and joints -->
    <!-- ####################################### -->
    <!-- Define base_link -->

    <!-- <link name="" /> -->
    <!-- <joint name="world_to_base_link" type="fixed"> -->
        <!-- <parent link="world" /> -->
        <!-- <child link="base_link"/> -->
        <!-- <origin rpy="0 0 0" xyz="0 0 0" /> -->
    <!-- </joint> -->




    <link name="base_link_inertia">
        <visual>
            <origin rpy="0 0 0.001" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/base.stl" />
            </geometry>
            <material name="black" />
        </visual>
        <collision name="base_link_collision">
            <origin rpy="0 0 0" xyz="0 0 0.001" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/base.stl" />
            </geometry>
        </collision>
        <inertial>
            <origin xyz="-0.0727 0.0048 0.0922" rpy="0 0 0" />
            <mass value="25.0" />
            <inertia ixx="0.1338" ixy="0.0010" ixz="0.0060" iyy="0.2240" iyz="0.0001" izz="0.2082" />
        </inertial>
    </link>
    <joint name="base_link_to_base_link_inertia" type="fixed">
        <parent link="base_link" />
        <child link="base_link_inertia" />
        <origin rpy="0 0 0" xyz="0 0 0" />
        <!-- <dynamics damping="0.5" friction="0.0" /> -->
    </joint>
    <!-- Define  upper_arm link -->
    <link name="upper_arm_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/upper_arm.stl" />
            </geometry>
            <material name="yellow" />
        </visual>
        <collision name="upper_arm_collision">
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/upper_arm.stl" />
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.1944 -0.0010 0.0301" rpy="0 0 0" />
            <mass value="1.5" />
            <inertia ixx="0.0021" ixy="-0.0000" ixz="0.0006" iyy="0.0178" iyz="0.0000" izz="0.0190" />
        </inertial>
    </link>
    <!-- Define shoulder joint -->
    <joint name="j1_joint" type="revolute">
        <parent link="base_link_inertia" />
        <child link="upper_arm_link" />
        <origin rpy="0 0 0" xyz="0 0 0.182" />
        <axis xyz="0 0 1" />
        <limit effort="100.0" lower="-2.50" upper="2.50" velocity="4.5" />
        <!-- <dynamics damping="0.5" friction="0.0" /> -->
    </joint>
    <!-- Define  forearm link -->
    <link name="forearm_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/forearm.stl" />
            </geometry>
            <material name="yellow" />
        </visual>
        <collision name="upper_arm_collision">
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/forearm.stl" />
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0.1511 0.0006 0.0958" rpy="0 0 0" />
            <mass value="2.5" />
            <inertia ixx="0.0089" ixy="0.0000" ixz="0.0012" iyy="0.0268" iyz="-0.0000" izz="0.0231" />
        </inertial>
    </link>
    <!-- Define elbow joint -->
    <joint name="j2_joint" type="revolute">
        <parent link="upper_arm_link" />
        <child link="forearm_link" />
        <origin rpy="0 0 0" xyz="0.35 0 0.022" />
        <axis xyz="0 0 1" />
        <limit effort="100.0" lower="-2.50" upper="2.50" velocity="4.5" />
        <!-- <dynamics damping="0.5" friction="0.0" /> -->
    </joint>
    <!-- Define vertical prismatic link -->
    <link name="wrist_link">
        <inertial>
            <mass value="0.5" />
            <origin rpy="0 0 0" xyz="0 0 0" />
            <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
            <!-- <inertia ixx="0.002" ixy="0.0" ixz="0.0" iyy="0.002" iyz="0.0" izz="0.02" /> -->
        </inertial>
    </link>
    <!-- Define prismatic vertical  joint -->
    <joint name="j3_joint" type="prismatic">
        <parent link="forearm_link" />
        <child link="wrist_link" />
        <origin rpy="0 3.14 0" xyz="0.30 0 0" />
        <axis xyz="0 0 1" />
        <limit effort="100.0" lower="0.05" upper="0.265" velocity="4.5" />
    </joint>
    <!-- Define end_effector link -->
    <link name="end_effector_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/end_effector.stl" />
            </geometry>
            <material name="dark_grey" />
        </visual>
        <collision name="upper_arm_collision">
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://spjbot_description/meshes/fanuc_scara/end_effector.stl" />
            </geometry>
        </collision>
        <inertial>
            <origin xyz="-0.0002 -0.0000 -0.2209" rpy="0 0 0" />
            <mass value="0.5" />
            <inertia ixx="0.0029" ixy="-0.0000" ixz="0.0000" iyy="0.0029" iyz="-0.0000" izz="0.0000" />
            <!-- <inertia ixx="0.002" ixy="0.0" ixz="0.0" iyy="0.002" iyz="0.0" izz="0.02" /> -->
        </inertial>
    </link>
    <!-- Define rotation of end effector joint -->
    <joint name="j4_joint" type="revolute">
        <parent link="wrist_link" />
        <child link="end_effector_link" />
        <origin rpy="0 0 0" xyz="0 0 0.0" />
        <axis xyz="0 0 1" />
        <limit effort="100.0" lower="-3.14" upper="3.14" velocity="4.5" />
    </joint>


    <!-- hand camera -->
    <xacro:rgb_camera parent="end_effector_link" prefix="rgb_hand" color_gazebo="Blue">
        <origin xyz="-0.025 0 0.02" rpy="0.0 -${M_PI/2.3} 0" />
        <color_rgb_rviz>
            <material name="blue">
                <color rgba="0.0 0.4 0.95 1" />
            </material>
        </color_rgb_rviz>
    </xacro:rgb_camera>




    <gazebo reference="base_link">
        <material>Gazebo/Black</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>10000000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
        <plugin name="gazebo_ros_control">
            <robotNamespace>/</robotNamespace>
        </plugin>
    </gazebo>
    <gazebo reference="base_link_inertia">
        <material>Gazebo/Black</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>10000000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="upper_arm_link">
        <material>Gazebo/DarkYellow</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>1000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="forearm_link">
        <material>Gazebo/DarkYellow</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>1000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="wrist_link">
        <material>Gazebo/DarkYellow</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>1000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo reference="end_effector_link">
        <material>Gazebo/Grey</material>
        <mu1>1000</mu1>
        <mu2>1000</mu2>
        <kp>1000.0</kp>
        <kd>1.0</kd>
        <selfCollide>true</selfCollide>
    </gazebo>
    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
            <robotNamespace>/</robotNamespace>
        </plugin>
    </gazebo>
    <transmission name="trans_j1_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="j1_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="j1_joint_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_j2_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="j2_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="j2_joint_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_j3_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="j3_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="j3_joint_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_j4_joint">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="j4_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="j4_joint_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
</robot>
