<!-- Add damping = 0.3, 0.1 for bigger ones -->
<!-- Set kp to larger -->
<mujoco model="s-model_articulated">
    <asset>
        <mesh name="palm" file="meshes/robotiq_s_gripper/palm.stl" />
        <mesh name="link_0" file="meshes/robotiq_s_gripper/link_0.stl" />
        <mesh name="link_1" file="meshes/robotiq_s_gripper/link_1.stl" />
        <mesh name="link_2" file="meshes/robotiq_s_gripper/link_2.stl" />
        <mesh name="link_3" file="meshes/robotiq_s_gripper/link_3.stl" />
        <mesh name="palm_vis" file="meshes/robotiq_s_gripper/palm_vis.stl" />
        <mesh name="link_0_vis" file="meshes/robotiq_s_gripper/link_0_vis.stl" />
        <mesh name="link_1_vis" file="meshes/robotiq_s_gripper/link_1_vis.stl" />
        <mesh name="link_2_vis" file="meshes/robotiq_s_gripper/link_2_vis.stl" />
        <mesh name="link_3_vis" file="meshes/robotiq_s_gripper/link_3_vis.stl" />
    </asset>
    <tendon>
		<!--Middlefinger tendons-->
		<fixed name="middle12_cpl" range="0 1">
			<joint joint="finger_middle_joint_1"  coef="0.5"/>
			<joint joint="finger_middle_joint_2"  coef="0.5"/>
		</fixed>
		<fixed name="middle23_cpl" range="0 1">
			<joint joint="finger_middle_joint_2"  coef="0.5"/>
			<joint joint="finger_middle_joint_3"  coef="-1"/>
		</fixed>

		<!--finger2 tendons-->
        <fixed name="finger2_12_cpl" range="0 1">
			<joint joint="finger_2_joint_1"  coef="0.5"/>
			<joint joint="finger_2_joint_2"  coef="0.5"/>
		</fixed>
		<fixed name="finger2_23_cpl" range="0 1">
			<joint joint="finger_2_joint_2"  coef="0.5"/>
			<joint joint="finger_2_joint_3"  coef="-1"/>
		</fixed>

		<!--Finger1 tendons-->
        <fixed name="finger1_12_cpl" range="0 1">
			<joint joint="finger_1_joint_1"  coef="0.5"/>
			<joint joint="finger_1_joint_2"  coef="0.5"/>
		</fixed>
		<fixed name="finger1_23_cpl" range="0 1">
			<joint joint="finger_1_joint_2"  coef="0.5"/>
			<joint joint="finger_1_joint_3"  coef="-1"/>
		</fixed>
	</tendon>

    <equality>
		<!-- GRIPPER Couplings -->
		<tendon name="middle12_cpl" 	tendon1="middle12_cpl"/>
		<tendon name="middle23_cpl" 	tendon1="middle23_cpl"/>

        <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"/>

		<!-- SCISSOR Coupling -->
		<joint name="finger_12_scissor" joint1="palm_finger_2_joint" joint2="palm_finger_1_joint" polycoef="0 -1 0 0 0"/>
	</equality>
    <actuator>
        <position name='finger_1' ctrllimited="true" kp="20" joint='finger_1_joint_1' ctrlrange='0 1.2217'  />
        <position name='finger_2' ctrllimited="true" kp="20" joint='finger_2_joint_1' ctrlrange='0 1.2217'  />
        <position name='middle_finger'  ctrllimited="true" kp="20" joint='finger_middle_joint_1' ctrlrange='0 1.2217'  />
        <position name='finger_scissor' ctrllimited="true" kp="20" joint='palm_finger_1_joint' ctrlrange="-0.29 0.29"  />
    </actuator>
    <!--
    <actuator>
        <position ctrllimited="true" ctrlrange="-0.16 0.25" joint="palm_finger_1_joint" kp="20" name="palm_finger_1_joint" forcelimited="true" forcerange="-500 500"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_1_joint_1" kp="2" name="finger_1_joint_1" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_1_joint_2" kp="2" name="finger_1_joint_2" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_1_joint_3" kp="2" name="finger_1_joint_3" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="-0.16 0.25" joint="palm_finger_2_joint" kp="20" name="palm_finger_2_joint" forcelimited="true" forcerange="-500 500"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_2_joint_1" kp="2" name="finger_2_joint_1" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_2_joint_2" kp="2" name="finger_2_joint_2" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_2_joint_3" kp="2" name="finger_2_joint_3" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_middle_joint_1" kp="2" name="finger_middle_joint_1" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_middle_joint_2" kp="2" name="finger_middle_joint_2" forcelimited="true" forcerange="-20 20"/>
        <position ctrllimited="true" ctrlrange="0 3.1416" joint="finger_middle_joint_3" kp="2" name="finger_middle_joint_3" forcelimited="true" forcerange="-20 20"/>
    </actuator>
    -->
    <worldbody>
        <body name="palm" pos="0 0 0.045" quat="0.49921826 0.50133955 0.50133955 0.49921826" >
            <site name="ft_frame" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 1" type="sphere" group="1"/>
            <inertial pos="-0.00134236 0.00263073 -1.01672e-08" quat="0.489887 0.489849 0.509908 0.509954" mass="1.20044" diaginertia="0.00263541 0.00218876 0.00210987" />
            <geom name="palm_visual" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="palm_vis" />
            <geom name="palm" contype="0" type="mesh" rgba="0 0 0 1" group="0" mesh="palm" />
            <site name="grip_site" pos="0 0.1 0" size="0.01" rgba="1 0 0 1" type="sphere" group="1"/>
            <site name="grip_site_cylinder" quat="0 0 0.707 0.707" pos="0 0 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/>
            <body name="finger_1_link_0" pos="-0.0455 0.0214 0.036" quat="-2.59838e-06 0.706825 0.707388 2.59631e-06">
                <inertial pos="-0.0381134 0.026928 0.0359997" quat="0.0835458 0.702169 -0.0835699 0.702136" mass="0.0494545" diaginertia="1.39633e-05 1.10443e-05 9.98401e-06" />
                <joint damping="0.1" name="palm_finger_1_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.29 0.29" ref="0.29" />
                <geom name="f1_l0_vis" pos="0.02 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_0_vis" />
                <geom name="f1_l0" contype="0" pos="0.02 0 0" type="mesh" rgba="0 0 0 1" group="0" mesh="link_0" />
                <!-- pos="-0.0455 0.0414 0.036" quat="-2.59838e-06 0.706825 0.707388 2.59631e-06"  -->
                <body name="finger_1_link_1" pos="0.02 0 0">
                    <inertial pos="0.0184407 -0.0219985 -5.83379e-12" quat="0.170512 0.68624 -0.170512 0.68624" mass="0.0859984" diaginertia="6.46024e-05 5.9524e-05 1.50124e-05" />
                    <joint damping="0.03" name="finger_1_joint_1" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                    <geom name="f1_l1_vis" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_1_vis" />
                    <geom name="f1_l1" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" rgba="0 0 0 1" group="0" mesh="link_1" />
                    <body name="finger_1_link_2" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081">
                        <inertial pos="0.0147516 -0.00823285 -0.000546645" quat="-0.0619175 0.704391 0.0619175 0.704391" mass="0.0461767" diaginertia="1.70717e-05 1.67626e-05 5.90767e-06" />
                        <joint damping="0.03" name="finger_1_joint_2" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                        <geom name="f1_l2_vis" pos="0.039 0 0.0075" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_2_vis" />
                        <geom name="f1_l2" pos="0.039 0 0.0075" type="mesh" rgba="0 0 0 1" group="0" mesh="link_2" />
                        <body name="finger_1_link_3" pos="0.039 0 0">
                            <inertial pos="0.00866021 0.00239356 2.11103e-08" quat="0.653623 0.653626 -0.269769 -0.269767" mass="0.0227212" diaginertia="5.48529e-06 4.69221e-06 2.01594e-06" />
                            <joint damping="0.03" name="finger_1_joint_3" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                            <geom name="f1_l3_vis" quat="0.96639 0 0 0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1" mesh="link_3_vis" />
                            <geom name="f1_l3" quat="0.96639 0 0 0.257081" type="mesh" rgba="1 1 1 1" group="0" mesh="link_3" />
                            <geom type="box" pos="0.018 0.018 0" quat="0.9659258 0 0 0.258819" size="0.013 0.002 0.013" name="f1_pad_collision" />
                        </body>
                    </body>
                </body>
            </body>
            <body name="finger_2_link_0" pos="-0.0455 0.0214 -0.036" quat="-2.59838e-06 0.706825 0.707388 2.59631e-06">
                <inertial pos="-0.0381134 0.026928 0.0359997" quat="0.0835458 0.702169 -0.0835699 0.702136" mass="0.0494545" diaginertia="1.39633e-05 1.10443e-05 9.98401e-06" />
                <joint damping="0.1" name="palm_finger_2_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-0.29 0.29" ref="-0.29" />
                <geom pos="0.02 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_0_vis" />
                <geom name="f2_l0" contype="0" pos="0.02 0 0" type="mesh" rgba="0 0 0 1" group="0" mesh="link_0" />
                <!-- pos="-0.0455 0.0414 0.036" quat="-2.59838e-06 0.706825 0.707388 2.59631e-06" type="mesh"  -->
                <body name="finger_2_link_1" pos="0.02 0 0">
                    <inertial pos="0.0184407 -0.0219985 -5.83379e-12" quat="0.170512 0.68624 -0.170512 0.68624" mass="0.0859984" diaginertia="6.46024e-05 5.9524e-05 1.50124e-05" />
                    <joint damping="0.03" name="finger_2_joint_1" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                    <geom pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_1_vis" />
                    <geom name="f2_l1" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" rgba="0 0 0 1" group="0" mesh="link_1" />
                    <body name="finger_2_link_2" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081">
                        <inertial pos="0.0147516 -0.00823285 -0.000546645" quat="-0.0619175 0.704391 0.0619175 0.704391" mass="0.0461767" diaginertia="1.70717e-05 1.67626e-05 5.90767e-06" />
                        <joint damping="0.03" name="finger_2_joint_2" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                        <geom pos="0.039 0 0.0075" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_2_vis" />
                        <geom name="f2_l2" pos="0.039 0 0.0075" type="mesh" rgba="0 0 0 1" group="0" mesh="link_2" />
                        <body name="finger_2_link_3" pos="0.039 0 0">
                            <inertial pos="0.00866021 0.00239356 2.11103e-08" quat="0.653623 0.653626 -0.269769 -0.269767" mass="0.0227212" diaginertia="5.48529e-06 4.69221e-06 2.01594e-06" />
                            <joint damping="0.03" name="finger_2_joint_3" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                            <geom quat="0.96639 0 0 0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1" mesh="link_3_vis" />
                            <geom name="f2_l3" quat="0.96639 0 0 0.257081" type="mesh" rgba="1 1 1 1" group="0" mesh="link_3" />
                            <geom type="box" pos="0.018 0.018 0" quat="0.9659258 0 0 0.258819" size="0.013 0.002 0.013" name="f2_pad_collision" />
                        </body>
                    </body>
                </body>
            </body>
            <body name="finger_middle_link_0" pos="0.0455 0.0214 0" quat="0.707388 0 0 0.706825">
                <inertial pos="-0.0381134 0.026928 0.0359997" quat="0.0835458 0.702169 -0.0835699 0.702136" mass="0.0494545" diaginertia="1.39633e-05 1.10443e-05 9.98401e-06" />
                <geom pos="0.02 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_0_vis" />
                <geom name="f3_l0" contype="0" pos="0.02 0 0" type="mesh" rgba="0 0 0 1" group="0" mesh="link_0" />
                <!-- pos="-0.0455 0.0414 0.036" quat="-2.59838e-06 0.706825 0.707388 2.59631e-06"  -->
                <body name="finger_middle_link_1" pos="0.02 0 0">
                    <inertial pos="0.0184407 -0.0219985 -5.83379e-12" quat="0.170512 0.68624 -0.170512 0.68624" mass="0.0859984" diaginertia="6.46024e-05 5.9524e-05 1.50124e-05" />
                    <joint damping="0.03" name="finger_middle_joint_1" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                    <geom pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_1_vis" />
                    <geom name="f3_l1" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081" type="mesh" rgba="0 0 0 1" group="0" mesh="link_1" />
                    <body name="finger_middle_link_2" pos="0.05 -0.028 0" quat="0.96639 0 0 -0.257081">
                        <inertial pos="0.0147516 -0.00823285 -0.000546645" quat="-0.0619175 0.704391 0.0619175 0.704391" mass="0.0461767" diaginertia="1.70717e-05 1.67626e-05 5.90767e-06" />
                        <joint damping="0.03" name="finger_middle_joint_2" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                        <geom pos="0.039 0 0.0075" type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="link_2_vis" />
                        <geom name="f3_l2" pos="0.039 0 0.0075" type="mesh" rgba="0 0 0 1" group="0" mesh="link_2" />
                        <body name="finger_middle_link_3" pos="0.039 0 0">
                            <inertial pos="0.00866021 0.00239356 2.11103e-08" quat="0.653623 0.653626 -0.269769 -0.269767" mass="0.0227212" diaginertia="5.48529e-06 4.69221e-06 2.01594e-06" />
                            <joint damping="0.03" name="finger_middle_joint_3" pos="0 0 0" axis="0 0 1" limited="true" range="0 3.1416" />
                            <geom quat="0.96639 0 0 0.257081" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1" mesh="link_3_vis" />
                            <geom  name="f3_l3" quat="0.96639 0 0 0.257081" type="mesh" rgba="1 1 1 1" group="0" mesh="link_3" />
                            <geom type="box" pos="0.018 0.018 0" quat="0.9659258 0 0 0.258819" size="0.013 0.002 0.013" name="finger_middle_pad_collision" />
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </worldbody>
    <sensor>
        <force name="force_ee" site="ft_frame"/>
        <torque name="torque_ee" site="ft_frame"/>
    </sensor>
    <!--
    <contact>
        <exclude body1="finger_1_link_1" body2="palm"/>
        <exclude body1="finger_2_link_1" body2="palm"/>
        <exclude body1="finger_middle_link_1" body2="palm"/>
        <exclude body1="finger_1_link_0" body2="palm"/>
        <exclude body1="finger_2_link_0" body2="palm"/>
        <exclude body1="finger_middle_link_0" body2="palm"/>
        <exclude body1="finger_1_link_0" body2="finger_1_link_1"/>
        <exclude body1="finger_2_link_0" body2="finger_2_link_1"/>
        <exclude body1="finger_middle_link_0" body2="finger_middle_link_1"/>
    </contact>
    -->
</mujoco>
