<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
    <xacro:macro name="robotiq_gripper" params="
        name
        prefix
        parent
        *origin
        include_ros2_control:=true
        use_fake_hardware:=false
        fake_sensor_commands:=false
        robot_ip:=192.168.1.1
        robot_port:=63352>

        <!-- ros2 control include -->
        <xacro:include filename="$(find robotiq_2f_description)/urdf/2f_140.ros2_control.xacro" />
        <!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
        <xacro:if value="${include_ros2_control}">
            <xacro:robotiq_gripper_ros2_control
                name="${name}" prefix="${prefix}"
                use_fake_hardware="${use_fake_hardware}"
                fake_sensor_commands="${fake_sensor_commands}"
                robot_ip="${robot_ip}"
                robot_port="${robot_port}"/>
        </xacro:if>

        <!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
        <link name="${prefix}robotiq_base_link"/>
        <joint name="${prefix}robotiq_base_joint" type="fixed">
          <parent link="${parent}" />
          <child link="${prefix}robotiq_base_link" />
          <origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
        </joint>

        <link name="${prefix}robotiq_140_base_link">
            <inertial>
                <origin xyz="8.625E-08 -4.6583E-06 0.03145" rpy="0 0 0" />
                <mass value="0.22652" />
                <inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_base_link.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_base_link.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_left_outer_knuckle">
            <inertial>
                <origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
                <mass value="0.00853198276973456" />
                <inertia
                    ixx="2.89328108496468E-06"
                    ixy="-1.57935047237397E-19"
                    ixz="-1.93980378593255E-19"
                    iyy="1.86719750325683E-06"
                    iyz="-1.21858577871576E-06"
                    izz="1.21905238907251E-06" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_right_outer_knuckle">
            <inertial>
                <origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
                <mass value="0.00853198276973456" />
                <inertia
                    ixx="2.89328108496468E-06"
                    ixy="-1.57935047237397E-19"
                    ixz="-1.93980378593255E-19"
                    iyy="1.86719750325683E-06"
                    iyz="-1.21858577871576E-06"
                    izz="1.21905238907251E-06" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
                </material>
            </visual>
            <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_left_outer_finger">
            <inertial>
                <origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
                <mass value="0.022614240507152" />
                <inertia
                    ixx="1.52518312458174E-05"
                    ixy="9.76583423954399E-10"
                    ixz="-5.43838577022588E-10"
                    iyy="6.17694243867776E-06"
                    iyz="6.78636130740228E-06"
                    izz="1.16494917907219E-05" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_right_outer_finger">
            <inertial>
                <origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
                <mass value="0.022614240507152" />
                <inertia
                    ixx="1.52518312458174E-05"
                    ixy="9.76583423954399E-10"
                    ixz="-5.43838577022588E-10"
                    iyy="6.17694243867776E-06"
                    iyz="6.78636130740228E-06"
                    izz="1.16494917907219E-05" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_left_inner_knuckle">
            <inertial>
                <origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
                <mass value="0.0271177346495152" />
                <inertia
                    ixx="2.61910379223783E-05"
                    ixy="-2.43616858946494E-07"
                    ixz="-6.37789906117123E-09"
                    iyy="2.8270243746167E-06"
                    iyz="-5.37200748039765E-07"
                    izz="2.83695868220296E-05" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_right_inner_knuckle">
            <inertial>
                <origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
                <mass value="0.0271177346495152" />
                <inertia
                    ixx="2.61910379223783E-05"
                    ixy="-2.43616858946494E-07"
                    ixz="-6.37789906117123E-09"
                    iyy="2.8270243746167E-06"
                    iyz="-5.37200748039765E-07"
                    izz="2.83695868220296E-05" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_left_inner_finger">
            <inertial>
                <origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
                <mass value="0.0104003125914103" />
                <inertia
                    ixx="2.71909453810972E-06"
                    ixy="1.35402465472579E-21"
                    ixz="-7.1817349065269E-22"
                    iyy="7.69100314106116E-07"
                    iyz="6.74715432769696E-07"
                    izz="2.30315190420171E-06" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_right_inner_finger">
            <inertial>
                <origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
                <mass value="0.0104003125914103" />
                <inertia
                    ixx="2.71909453810972E-06"
                    ixy="1.35402465472579E-21"
                    ixz="-7.1817349065269E-22"
                    iyy="7.69100314106116E-07"
                    iyz="6.74715432769696E-07"
                    izz="2.30315190420171E-06" />
            </inertial>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
                </geometry>
                <material name="">
                    <color rgba="0.1 0.1 0.1 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
                </geometry>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_left_inner_finger_pad">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.027 0.065 0.0075"/>
                </geometry>
                <material name="">
                    <color rgba="0.9 0.9 0.9 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.03 0.07 0.0075"/>
                </geometry>
                <material name="">
                    <color rgba="0.9 0.0 0.0 1" />
                </material>
            </collision>
        </link>

        <link name="${prefix}robotiq_140_right_inner_finger_pad">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.027 0.065 0.0075"/>
                </geometry>
                <material name="">
                    <color rgba="0.9 0.9 0.9 1" />
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.03 0.07 0.0075"/>
                </geometry>
                <material name="">
                    <color rgba="0.9 0.0 0.0 1" />
                </material>
            </collision>
        </link>



        <!-- Joints-->
        <joint name="${prefix}robotiq_140_base_joint" type="fixed">
            <parent link="${prefix}robotiq_base_link" />
            <child link="${prefix}robotiq_140_base_link" />
            <xacro:insert_block name="origin" />
        </joint>

        <joint name="${prefix}finger_joint" type="revolute">
            <origin xyz="0 -0.030601 0.054905" rpy="${pi / 2 + .725} 0 0" />
            <parent link="${prefix}robotiq_140_base_link" />
            <child link="${prefix}robotiq_140_left_outer_knuckle" />
            <axis xyz="-1 0 0" />
            <limit lower="0" upper="0.7" velocity="2.0" effort="1000" />
        </joint>

        <joint name="${prefix}right_outer_knuckle_joint" type="revolute">
            <origin xyz="0 0.030601 0.054905" rpy="${pi / 2 + .725} 0 ${pi}" />
            <parent link="${prefix}robotiq_140_base_link" />
            <child link="${prefix}robotiq_140_right_outer_knuckle" />
            <axis xyz="1 0 0" />
            <limit lower="-0.725" upper="0.725" velocity="2.0" effort="1000" />
            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
        </joint>

        <joint name="${prefix}left_outer_finger_joint" type="fixed">
            <origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
            <parent link="${prefix}robotiq_140_left_outer_knuckle" />
            <child link="${prefix}robotiq_140_left_outer_finger" />
            <axis xyz="1 0 0" />
        </joint>

        <joint name="${prefix}right_outer_finger_joint" type="fixed">
            <origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
            <parent link="${prefix}robotiq_140_right_outer_knuckle" />
            <child link="${prefix}robotiq_140_right_outer_finger" />
            <axis xyz="1 0 0" />
        </joint>

        <joint name="${prefix}left_inner_knuckle_joint" type="revolute">
            <origin xyz="0 -0.0127 0.06142" rpy="${pi / 2 + .725} 0 0" />
            <parent link="${prefix}robotiq_140_base_link" />
            <child link="${prefix}robotiq_140_left_inner_knuckle" />
            <axis xyz="1 0 0" />
            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
        </joint>

        <joint name="${prefix}right_inner_knuckle_joint" type="revolute">
            <origin xyz="0 0.0127 0.06142" rpy="${pi / 2 + .725} 0 ${-pi}" />
            <parent link="${prefix}robotiq_140_base_link" />
            <child link="${prefix}robotiq_140_right_inner_knuckle" />
            <axis xyz="1 0 0" />
            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
        </joint>

        <joint name="${prefix}left_inner_finger_joint" type="revolute">
            <origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
            <parent link="${prefix}robotiq_140_left_outer_finger" />
            <child link="${prefix}robotiq_140_left_inner_finger" />
            <axis xyz="1 0 0" />
            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
            <mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
        </joint>

        <joint name="${prefix}right_inner_finger_joint" type="revolute">
            <origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
            <parent link="${prefix}robotiq_140_right_outer_finger" />
            <child link="${prefix}robotiq_140_right_inner_finger" />
            <axis xyz="1 0 0" />
            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
            <mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
        </joint>

        <joint name="${prefix}left_inner_finger_pad_joint" type="fixed">
            <origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
            <parent link="${prefix}robotiq_140_left_inner_finger" />
            <child link="${prefix}robotiq_140_left_inner_finger_pad" />
            <axis xyz="0 0 1" />
        </joint>

        <joint name="${prefix}right_inner_finger_pad_joint" type="fixed">
            <origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
            <parent link="${prefix}robotiq_140_right_inner_finger" />
            <child link="${prefix}robotiq_140_right_inner_finger_pad" />
            <axis xyz="0 0 1" />
        </joint>
    </xacro:macro>
</robot>
