<mujoco model="robotiq_gripper_140_model">
    <asset>
        <mesh name="robotiq_arg2f_base_link" file="meshes/robotiq_arg2f_base_link.stl" />
        <mesh name="robotiq_arg2f_140_outer_knuckle" file="meshes/robotiq_arg2f_140_outer_knuckle.stl" />
        <mesh name="robotiq_arg2f_140_outer_finger" file="meshes/robotiq_arg2f_140_outer_finger.stl" />
        <mesh name="robotiq_arg2f_140_inner_finger" file="meshes/robotiq_arg2f_140_inner_finger.stl" />
        <mesh name="robotiq_arg2f_140_inner_knuckle" file="meshes/robotiq_arg2f_140_inner_knuckle.stl" />
        <mesh name="robotiq_arg2f_base_link_vis" file="meshes/robotiq_arg2f_base_link_vis.stl" />
        <mesh name="robotiq_arg2f_140_outer_knuckle_vis" file="meshes/robotiq_arg2f_140_outer_knuckle_vis.stl" />
        <mesh name="robotiq_arg2f_140_outer_finger_vis" file="meshes/robotiq_arg2f_140_outer_finger_vis.stl" />
        <mesh name="robotiq_arg2f_140_inner_finger_vis" file="meshes/robotiq_arg2f_140_inner_finger_vis.stl" />
        <mesh name="robotiq_arg2f_140_inner_knuckle_vis" file="meshes/robotiq_arg2f_140_inner_knuckle_vis.stl" />
    </asset>

    <tendon>
		<!--finger2 tendons-->
        <fixed name="finger2_12_cpl" >
			<joint joint="finger_joint"  coef="1"/>
			<joint joint="left_inner_finger_joint"  coef="1.5"/>
		</fixed>
		<fixed name="finger2_23_cpl" >
			<joint joint="left_inner_finger_joint"  coef="1"/>
			<joint joint="left_inner_knuckle_joint"  coef="3.5"/>
		</fixed>

		<!--Finger1 tendons-->
        <fixed name="finger1_12_cpl" >
			<joint joint="right_outer_knuckle_joint"  coef="1"/>
			<joint joint="right_inner_finger_joint"  coef="-1.5"/>
		</fixed>
		<fixed name="finger1_23_cpl" >
			<joint joint="right_inner_finger_joint"  coef="1"/>
			<joint joint="right_inner_knuckle_joint"  coef="3.5"/>
		</fixed>
	</tendon>

    <equality>
		<!-- GRIPPER Couplings -->
        <tendon name="finger2_12_cpl" 	tendon1="finger2_12_cpl"/>
		<tendon name="finger2_23_cpl" 	tendon1="finger2_23_cpl"/>

        <tendon name="finger1_12_cpl" 	tendon1="finger1_12_cpl"/>
		<tendon name="finger1_23_cpl" 	tendon1="finger1_23_cpl"/>
        
	</equality>
    <!-- <equality>
        <joint joint1="finger_joint" joint2="right_outer_knuckle_joint"/>
    </equality> -->

    <actuator>
        <position name='finger_1' ctrllimited="true" kp="20" joint='finger_joint' ctrlrange='0 1' />
        <position name='finger_2' ctrllimited="true" kp="20" joint='right_outer_knuckle_joint' ctrlrange='0 1' />

        <!-- <position name="0_robotiq_2f_85" joint="0_robotiq_2f_85_right_driver_joint" ctrlrange="0 1" ctrllimited="true" /> -->
        <!-- <motor name="finger_1" ctrllimited="true" ctrlrange="-100.0 100.0" joint="0_j7" /> -->

    </actuator>

    <worldbody>
        <body name="right_gripper" pos="0 0 -0.0625" quat="0 -0.707105 0.707108 0 ">
            <geom pos="0 0 -0.061525" quat="0 0.707388 -0.706825 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="hand_visual" mesh="robotiq_arg2f_base_link_vis" />
            <geom pos="0 0 -0.061525" quat="0 0.707388 -0.706825 0" type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_base_link" name="hand_collision"/>

            <site name="ft_frame" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 1" type="sphere" group="1" />

            <!-- This site was added for visualization. -->
            <body name="eef" pos="0 0 -0.27" quat="0 1 0 0">
                <site name="grip_site" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 0.5" type="sphere" group="1"/>
                <site name="ee_x" pos="0.1 0 0" size="0.005 .1"  quat="0.707105  0 0.707108 0 " rgba="1 0 0 0" type="cylinder" group="1"/>
                <site name="ee_y" pos="0 0.1 0" size="0.005 .1" quat="0.707105 0.707108 0 0" rgba="0 1 0 0" type="cylinder" group="1"/>
                <site name="ee_z" pos="0 0 0.1" size="0.005 .1" quat="1 0 0 0" rgba="0 0 1 0" type="cylinder" group="1"/>
                <!-- This site was added for visualization. -->
                <!-- <site name="grip_site_cylinder" pos="0 0 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/> -->
            </body>

            <body name="left_outer_knuckle" pos="0.030601 2.43684e-05 -0.11643" quat="-0.64507 0.290316 -0.290085 0.644556">
                <inertial pos="0.000163875 0.0458404 0.0117804" quat="0.881368 0.472423 -0.0024451 -0.000996122" mass="0.0311462" diaginertia="2.96023e-05 2.79814e-05 4.39017e-06" />
                <joint name="finger_joint" pos="0 0 0" axis="-1 0 0" limited="true" range="0 0.7" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.792157 0.819608 0.933333 1" name="left_outer_knuckle_visual" mesh="robotiq_arg2f_140_outer_knuckle_vis" />
                <geom type="mesh" group="0" rgba="0.792157 0.819608 0.933333 1" mesh="robotiq_arg2f_140_outer_knuckle" name="left_outer_knuckle_collision"/>
                <geom pos="0 0.01822 0.0260018" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="left_outer_finger_visual" mesh="robotiq_arg2f_140_outer_finger_vis" />
                <geom pos="0 0.01822 0.0260018" type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_outer_finger" name="left_outer_finger_collision"/>
                <body name="left_inner_finger" pos="0 0.0999754 -0.00221853" quat="0.935013 -0.354613 0 0">
                    <inertial pos="0.000119314 0.0339244 -0.021841" quat="0.545437 0.430197 -0.442938 0.566776" mass="0.0261503" diaginertia="1.62408e-05 1.59131e-05 2.38936e-06" />
                    <joint name="left_inner_finger_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.8757 0.8757" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="left_inner_finger_visual" mesh="robotiq_arg2f_140_inner_finger_vis" />
                    <geom type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_inner_finger" name="left_inner_finger_collision"/>
                    <geom size="0.0135 0.0325 0.00375" pos="0 0.0457554 -0.0272203" type="box" contype="0" conaffinity="0" group="1" rgba="0.9 0.9 0.9 1" name="left_fingertip_visual" />
                    <geom size="0.015 0.035 0.00375" pos="0 0.0457554 -0.0272203" type="box" group="0" rgba="0.9 0.9 0.9 1" name="left_fingertip_collision" solref="0.01 0.25"/>
                    <geom size="0.014 0.033 0.001" pos="0 0.045 -0.031" type="box" group="0" name="left_fingerpad_collision" />
                </body>
            </body>
            <body name="left_inner_knuckle" pos="0.0127 1.01133e-05 -0.122945" quat="-0.64507 0.290316 -0.290085 0.644556">
                <inertial pos="0.000123012 0.0507851 0.00103969" quat="0.497203 0.502496 -0.507943 0.492221" mass="0.0271177" diaginertia="2.83809e-05 2.61936e-05 2.81319e-06" />
                <joint name="left_inner_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.8757 0.8757" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="left_inner_knuckle_visual" mesh="robotiq_arg2f_140_inner_knuckle_vis" />
                <geom type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_inner_knuckle" name="left_inner_knuckle_collision"/>
            </body>
            <body name="right_outer_knuckle" pos="-0.030601 -2.43684e-05 -0.11643" quat="0.644556 -0.290085 -0.290316 0.64507">
                <inertial pos="0.000163875 0.0458404 0.0117804" quat="0.881368 0.472423 -0.0024451 -0.000996122" mass="0.0311462" diaginertia="2.96023e-05 2.79814e-05 4.39017e-06" />
                <joint name="right_outer_knuckle_joint" pos="0 0 0" axis="-1 0 0" limited="true" range="-0.725 0.725" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.792157 0.819608 0.933333 1" name="right_outer_knuckle_visual" mesh="robotiq_arg2f_140_outer_knuckle_vis" />
                <geom type="mesh" group="0" rgba="0.792157 0.819608 0.933333 1" mesh="robotiq_arg2f_140_outer_knuckle" name="right_outer_knuckle_collision"/>
                <geom pos="0 0.01822 0.0260018" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="right_outer_finger_visual" mesh="robotiq_arg2f_140_outer_finger_vis" />
                <geom pos="0 0.01822 0.0260018" type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_outer_finger" name="right_outer_finger_collision"/>
                <body name="right_inner_finger" pos="0 0.0999754 -0.00221853" quat="0.935013 -0.354613 0 0">
                    <inertial pos="0.000119314 0.0339244 -0.021841" quat="0.545437 0.430197 -0.442938 0.566776" mass="0.0261503" diaginertia="1.62408e-05 1.59131e-05 2.38936e-06" />
                    <joint name="right_inner_finger_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.8757 0.8757" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="right_inner_finger_visual" mesh="robotiq_arg2f_140_inner_finger_vis" />
                    <geom type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_inner_finger" name="right_inner_finger_collision"/>
                    <geom size="0.0135 0.0325 0.00375" pos="0 0.0457554 -0.0272203" type="box" contype="0" conaffinity="0" group="1" rgba="0.9 0.9 0.9 1" name="right_fingertip_visual" />
                    <geom size="0.015 0.035 0.00375" pos="0 0.0457554 -0.0272203" type="box" group="0" rgba="0.9 0.9 0.9 1" name="right_fingertip_collision" solref="0.01 0.25"/>
                    <geom size="0.014 0.033 0.001" pos="0 0.045 -0.031" type="box" group="0" name="right_fingerpad_collision" />
                </body>
            </body>
            <body name="right_inner_knuckle" pos="-0.0127 -1.01133e-05 -0.122945" quat="-0.644556 0.290085 0.290316 -0.64507">
                <inertial pos="0.000123012 0.0507851 0.00103969" quat="0.497203 0.502496 -0.507943 0.492221" mass="0.0271177" diaginertia="2.83809e-05 2.61936e-05 2.81319e-06" />
                <joint name="right_inner_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.8757 0.8757" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.1 0.1 0.1 1" name="right_inner_knuckle_visual" mesh="robotiq_arg2f_140_inner_knuckle_vis" />
                <geom type="mesh" group="0" rgba="0.1 0.1 0.1 1" mesh="robotiq_arg2f_140_inner_knuckle" name="right_inner_knuckle_collision"/>
            </body>
        </body>
    </worldbody>
    <sensor>
        <force name="force_ee" site="ft_frame"/>
        <torque name="torque_ee" site="ft_frame"/>
    </sensor>
</mujoco>
