<mujoco model="robotiq_2f_85">
    <compiler angle="radian" meshdir="../robotiq_2f_model/model/meshes/2f85/collision/"/>

  <visual>
    <scale constraint="0.01"/>
  </visual>
  <asset>
    <mesh name="base_mesh" file="base.stl" scale="0.001 0.001 0.001" />
    <mesh name="driver_mesh" file="driver.stl" scale="0.001 0.001 0.001" />
    <mesh name="coupler_mesh" file="coupler.stl" scale="0.001 0.001 0.001" />
    <mesh name="follower_v2_mesh" file="follower_v2.stl" />
    <mesh name="pad_mesh" file="fingertip_v2.stl" />
    <mesh name="silicon_pad_mesh" file="finger_pad_v2.stl" />
    <mesh name="fingertip_pad_v2_mesh" file="fingertip_pad_v2.stl" />
    <mesh name="reinforced_fingertip_mesh" file="reinforced_fingertip.stl" />
    <mesh name="spring_link_mesh" file="spring_link.stl" scale="0.001 0.001 0.001" />
    <mesh name="base_mount_mesh" file="base_mount.stl" scale="0.001 0.001 0.001" />
  </asset>

  <default>
    <geom type="mesh" conaffinity="7" contype="7" rgba=".149 .149 .149 1"/>
    <joint axis="1 0 0" limited="false"/>
    <site group="3" type="sphere" rgba=".9 .9 .9 1" size="0.005" />
    <default class="base_mount">
      <geom contype="2" conaffinity="1" type="mesh" mesh="base_mount_mesh" />
    </default>
    <default class="base">
      <geom contype="2" conaffinity="1" type="mesh" mesh="base_mesh" />
    </default>
    <default class="top_base">
      <geom contype="2" conaffinity="1" type="mesh" mesh="top_base_mesh" rgba=".58 .58 .58 1" />
    </default>
    <default class="spring_link">
      <geom contype="6" conaffinity="5" type="mesh" mesh="spring_link_mesh" />
      <joint limited="true" range="-0.29670597283 0.8" stiffness=".001" springref="2.62" damping="0.00125"/>
    </default>
    <default class="driver">
      <geom contype="2" conaffinity="1" type="mesh" mesh="driver_mesh" rgba=".4627 .4627 .4627 1" />
      <joint limited="true" range="0 0.8" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit=".005 1"/>
    </default>
    <default class="coupler">
      <geom contype="6" conaffinity="5" type="mesh" mesh="coupler_mesh" />
      <joint limited="true" range="-1.57 0." solimplimit="0.95 0.99 0.001" solreflimit=".005 1"/>
    </default>
    <default class="follower">
      <geom contype="2" conaffinity="1" type="mesh" mesh="follower_v2_mesh" />
      <joint limited="true" range="-0.872664 0.872664" pos="0 -0.018 0.0065" solimplimit="0.95 0.99 0.001" solreflimit=".005 1"/>
    </default>
    <default class="reinforced_fingertip">
      <geom condim="4" contype="6" conaffinity="5" rgba=".85 .85 .85 1." friction=".75 .75" solimp="0.95 0.99 0.001" solref=".002 1" />
    </default>
  </default>

  <worldbody>
    <body name="base_mount" pos="0 0 0.007" quat="0.707 0 0 -0.707">
      <geom name="base_mount_geom" class="base_mount" />
      <body name="base" pos="0 0 0.0038" quat="0.707 0 0 -0.707">
        <!-- Mass calculated from mesh = 0.275583 -->
        <!-- Gripper weighs 0.9 Kg, hence (mesh-estimated) mass of the other geoms was subtracted, resulting in the mass value below -->
        <inertial pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0" mass="0.777441" diaginertia="0.000260285 0.000225381 0.000152708" />
        <geom name="base_geom" class="base" />
        <site name="base_top_site" pos="0 0 0.088" />
        <site name="pinch_site" pos="0 0 0.1489" />

        <!-- Right-hand side 4-bar linkage -->
        <body name="right_driver" pos="0 0.0306011 0.054904">
          <inertial pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" mass="0.00899563" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07" />
          <joint name="right_driver_joint" class="driver" />
          <geom name="right_driver_geom" class="driver" />
          <body name="right_coupler" pos="0 0.0315 -0.0041">
            <inertial pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" mass="0.0140974" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07" />
            <joint name="right_coupler_joint" class="coupler" />
            <geom name="right_coupler_geom" class="coupler" />
          </body>
        </body>
        <body name="right_spring_link" pos="0 0.0132 0.0609">
          <inertial pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" mass="0.0221642" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06" />
          <joint name="right_spring_link_joint" class="spring_link" />
          <geom name="right_spring_link_geom" class="spring_link" />
          <body name="right_follower" pos="0 0.055 0.0375">
            <inertial pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" mass="0.0125222" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07" />
            <joint name="right_follower_joint" class="follower" />
            <geom name="right_follower_geom" class="follower" />
            <body name="right_pad" pos="0 -0.011401 0.013015" quat="0 0 0 1">
              <!-- DeepMind 3D-printed reinforced finger tip -->
              <geom name="reinforced_right_fingertip_geom" class="reinforced_fingertip" type="mesh" mesh="reinforced_fingertip_mesh"/>
            </body>
          </body>
        </body>
        <!-- Left-hand side 4-bar linkage -->
        <body name="left_driver" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
          <inertial pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0" mass="0.00899563" diaginertia="1.72352e-06 1.60906e-06 3.22006e-07" />
          <joint name="left_driver_joint" class="driver" />
          <geom name="left_driver_geom" class="driver" />
          <body name="left_coupler" pos="0 0.0315 -0.0041">
            <inertial pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636" mass="0.0140974" diaginertia="4.16206e-06 3.52216e-06 8.88131e-07" />
            <joint name="left_coupler_joint" class="coupler" />
            <geom name="left_coupler_geom" class="coupler" />
          </body>
        </body>
        <body name="left_spring_link" pos="0 -0.0132 0.0609" quat="0 0 0 1">
          <inertial pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403" mass="0.0221642" diaginertia="8.96853e-06 6.71733e-06 2.63931e-06" />
          <joint name="left_spring_link_joint" class="spring_link" />
          <geom name="left_spring_link_geom" class="spring_link" />
          <body name="left_follower" pos="0 0.055 0.0375">
            <inertial pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0" mass="0.0125222" diaginertia="2.67415e-06 2.4559e-06 6.02031e-07" />
            <joint name="left_follower_joint" class="follower" />
            <geom name="left_follower_geom" class="follower" />
            <body name="left_pad" pos="0 -0.011401 0.013015" quat="0 0 0 1">
              <!-- DeepMind 3D-printed reinforced finger tip -->
              <geom name="reinforced_left_fingertip_geom" class="reinforced_fingertip" type="mesh" mesh="reinforced_fingertip_mesh"/>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <!-- This adds stability to the model by having a tendon that distributes -->
  <!-- the forces between both joints, such that the equality constraint -->
  <!-- doesn't have to do that much work in order to equalize both joints. -->
  <!-- Since both joints share the same sign, we split the force between both -->
  <!-- equally by setting coef=0.5. -->
  <tendon>
    <fixed name="split">
      <joint joint="right_driver_joint" coef="0.5"/>
      <joint joint="left_driver_joint" coef="0.5"/>
    </fixed>
  </tendon>

  <equality>
    <connect anchor="0 0 0" body1="right_follower" body2="right_coupler" solimp="0.95 0.99 0.001" solref=".005 1"/>
    <connect anchor="0 0 0" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001" solref=".005 1"/>
    <joint joint1="right_driver_joint" joint2="left_driver_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001" solref=".005 1"/>
  </equality>

  <!-- The general actuator below is a customized position actuator (with some damping) where -->
  <!-- gainprm[0] != kp. See http://mujoco.org/book/modeling.html#position -->
  <!-- The reason why gainprm[0] != kp is because the control input range has to be re-scaled to [0, 255] -->
  <!-- The joint range is currently set at [0, 0.8], the control range is [0, 255] and kp = 100 -->
  <!-- Tau = Kp * scale * control_input - Kp * error -->
  <!-- max(Kp * scale * control_input) = 0.8, hence scale = 0.8 * 100 / 255 -->
  <actuator>
    <general name="fingers_actuator" tendon="split" ctrllimited="true" forcelimited="true" forcerange="-5. 5."
             ctrlrange="0 255" biastype="affine" gainprm="0.3137255 0 0" biasprm="0 -100 -10"/>
  </actuator>
  <sensor>
    <jointpos name="fingers_pos" joint="left_driver_joint" />
  </sensor>
</mujoco>
