<?xml version="1.0" ?>

<mujoco model="sawyer">
  <camera pos="0 0 3.2" name="topview" />
  <!-- <geom name="tableTop" type="plane" pos="0.8 0 0" size="0.4 0.4 0.5"
        rgba=".6 .6 .5 1" contype="1" conaffinity="1" friction="2 0.1 0.002" material="light_wood_v3"/> -->

  <body name="base" pos="0 0 0">
    <site name="basesite" pos="0 0 0" size="0.01" />
    <inertial pos="0 0 0" mass="0" diaginertia="0 0 0" />
    <body name="pedestal" pos="0 0 0">
      <inertial pos="-0.15 0 -0.2" mass="46.64" diaginertia="1.71363 1.27988 0.809981" />
      <geom size="0.3 0.3 0.2" pos="-0.19 0 -0.2" type="box" rgba="0.2 0.2 0.2 1"/>
    </body>
    <body name="kuka_link_0">
      <!-- <geom class="collision" type="mesh" mesh="kuka_link_0_mesh"/> -->
      <geom class="visual" type="mesh" mesh="kuka_link_0_mesh" material="kuka_grey"/>
      <body name="kuka_link_1" pos="0 0 0">
        <joint name="kuka_joint_1" axis="0 0 1" limited="true" range="-2.96705972839 2.96705972839" armature="1" damping="50" frictionloss="0" stiffness="10" />
        <inertial euler="0 0 0" pos="0 -0.035 0.271" mass="3.4821" diaginertia="0.02076 0.02179 0.00779"/>
        <geom class="visual" type="mesh" mesh="kuka_link_1_mesh" material="kuka_grey"/>
        <geom class="collision" type="mesh" mesh="kuka_link_1_mesh" />
        <body name="kuka_link_2"  pos="0 0 0">
          <joint name="kuka_joint_2" axis="0 1 0" limited="true" range="-2.09439510239 2.09439510239" pos="0 0 0.34" armature="1" damping="50" frictionloss="0" stiffness="10"/>
          <inertial euler="0 0 0" pos="0 0.34 0.407" mass="4.05623" diaginertia="0.03204 0.00972 0.03042"/>
          <geom class="collision" type="mesh" mesh="kuka_link_2_mesh"/>
          <geom class="visual" type="mesh" mesh="kuka_link_2_mesh" material="kuka_orange"/>
          <body name="kuka_link_3"  pos="0 0 0">
            <joint name="kuka_joint_3" axis="0 0 1" limited="true" range="-2.96705972839 2.96705972839" armature="1" damping="50" frictionloss="0" stiffness="10"/>
            <inertial euler="0 0 0" pos="0 0.03 0.651" mass="3.4822" diaginertia="0.02178 0.02075 0.007785"/>
            <geom class="collision" type="mesh" mesh="kuka_link_3_mesh"/>
            <geom class="visual" type="mesh" mesh="kuka_link_3_mesh" material="kuka_grey"/>
            <body name="kuka_link_4"  pos="0 0 0">
              <joint name="kuka_joint_4" axis="0 1 0" limited="true" range="-2.09439510239 2.09439510239" pos="0 0 0.74" armature="1" damping="50" frictionloss="0" stiffness="10"/>
              <inertial euler="0 0 0" pos="0 -0.034 0.807" mass="2.1633" diaginertia="0.01287  0.005708  0.01112"/>
              <geom class="collision" type="mesh" mesh="kuka_link_4_mesh"/>
              <geom class="visual" type="mesh" mesh="kuka_link_4_mesh" material="kuka_orange"/>
              <body name="kuka_link_5"  pos="0 0 0">
                <joint name="kuka_joint_5" axis="0 0 1" limited="true" range="-2.96705972839 2.96705972839" armature="1" damping="50" frictionloss="0" stiffness="10"/>
                <inertial euler="0 0 0" pos="0.0 -0.021 1" mass="2.3466" diaginertia="0.006509 0.006259 0.004527"/>
                <geom class="collision" type="mesh" mesh="kuka_link_5_mesh"/>
                <geom class="visual" type="mesh" mesh="kuka_link_5_mesh" material="kuka_grey"/>
                <body name="kuka_link_6"  pos="0 0 0">
                  <joint name="kuka_joint_6" axis="0 1 0" limited="true" range="-2.09439510239 2.09439510239" pos="0 0 1.14" armature="1" damping="50" frictionloss="0" stiffness="10"/>
                  <inertial euler="0 0 0" pos="0 0.0004 1.142" mass="2" diaginertia="0.01464 0.01465 0.002872"/>
                  <geom class="collision" type="mesh" mesh="kuka_link_6_mesh"/>
                  <geom class="visual" type="mesh" mesh="kuka_link_6_mesh" material="kuka_orange"/>
                  <body name="kuka_link_7"  pos="0 0 0" >
                    <joint name="kuka_joint_7" axis="0 0 1" limited="true" range="-3.05432619099 3.05432619099" armature="1" damping="50" frictionloss="0" stiffness="10"/>
                    <inertial euler="0 0 0" pos="0 0 1.241" mass="1.129" diaginertia="0.001 0.001 0.001"/>
                    <geom class="collision" type="mesh" mesh="kuka_link_7_mesh"/>
                    <geom class="visual" type="mesh" mesh="kuka_link_7_mesh" material="kuka_grey"/>


                    <!--  ================= BEGIN GRIPPER ================= /-->
                    <!--
                    <body name="gripper_entity" pos="0 0 1.22">
                      <body name="hand" euler="0 0 0" pos="0 0 0.05">
                        <site name="endEffector" pos="0 0 0" quat="1 0 0 0" size="0.005, 0.005, 0.005"/>
                        <inertial pos="0 0 0" mass="0.5"/>
                        <geom class="collision" type="cylinder" pos="0 0 0.01862" size="0.031 0.01862"/>
                        <geom class="collision" type="box" pos="0 0 0.042" size="0.02041 0.049 0.00476"/>

                        <body childclass="fetchGripper" name="r_gripper_finger_link" pos="0 0.0159 0.08526">

                          <joint axis="0 1 0" pos="0 0 0" name="r_gripper_finger_joint" range="0 0.035"/>

                          <geom pos="0 0 0" size="0.015 0.003 0.045" type="box" name="r_gripper_finger_link" condim="4" friction="1 0.05 0.01"/>

                          <site name="rightEndEffector" pos="0 0 0.03" size="0.01" rgba="1.0 0.0 0.0 1.0"/>
                          <body name="rightpad" pos ="0 .003 0" >
                            <geom name="rightpad_geom" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.015 0.003 0.045" rgba="1 1 1 1.0" solimp="0.9 0.9 0.01" solref="0.01 1" friction="2 0.1 0.002" contype="1" conaffinity="1"/>
                          </body>
                        </body>

                        <body childclass="fetchGripper" name="l_gripper_finger_link" pos="0 -0.0159 0.08526">

                          <joint axis="0 1 0" pos="0 0 0" name="l_gripper_finger_joint" range="-0.035 0"/>

                          <geom pos="0 0 0" size="0.015 0.003 0.045" type="box" name="l_gripper_finger_link" condim="4" friction="1 0.05 0.01"/>

                          <site name="leftEndEffector" pos="0 0 0.03" size="0.01" rgba="1.0 0.0 0.0 1.0"/>
                          <body name="leftpad" pos ="0 -.003 0" >
                            <geom name="leftpad_geom" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.015 0.003 0.045" rgba="0 1 1 1.0" solimp="0.9 0.9 0.01" solref="0.01 1" friction="2 0.1 0.002"  contype="1" conaffinity="1" />
                          </body>
                        </body>
                      </body>
                    </body>
                    -->
                    <!--  ================= END GRIPPER ================= /-->

                    <!-- new gripper setting -->
                    <body name="hand" pos="0 0 1.329" quat="-1 0 1 0">
                      <site name="endEffector" pos="0.04 0 0" size="0.01" rgba='1 1 1 0' />

                      <geom type="box" pos="-0.044 0 0" size="0.018 0.031 0.031" rgba="0.5 0.5 0.5 1.0" condim="3" friction=".6 0.005 0.0001"/>
                      <geom type="box" pos="-0.023 0 0" size="0.00476 0.049 0.02041" rgba="0.5 0.5 0.5 1.0" condim="3" friction=".6 0.005 0.0001"/>
                      <!--IMPORTANT: For rougher contact with gripper, set higher friciton values for the other interacting objects -->

                      <body name="rightclaw" pos="0.015 -0.05 0">

                        <geom name="rightclaw_it" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.0385 0.003 0.0135"  rgba="1 1 1 1.0"   />

                        <joint name="r_close" pos="0 0 0" axis="0 1 0" range= "0 0.04" armature="100" damping="1000" limited="true"  type="slide"/>
                        <!-- <joint name="r_close" pos="0 0 0" axis="0 1 0" range= "0 0.03" armature="100" damping="1000" limited="true"  type="slide"/>  -->

                        <!-- <site name="rightEndEffector" pos="0.0 0.005 0" size="0.044 0.008 0.012" type='box' /> -->

                        <!-- <site name="rightEndEffector" pos="0.035 0 0" size="0.01" rgba="1.0 0.0 0.0 1.0"/> -->
                        <site name="rightEndEffector" pos="0.0385 0 0" size="0.01" rgba="1.0 0.0 0.0 1.0"/>
                        <body name="rightpad" pos ="0 .003 0" >
                          <geom name="rightpad_geom" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.0385 0.003 0.0135" rgba="1 1 1 1.0" solimp="0.9 0.9 0.01" solref="0.01 1" friction="2 0.1 0.002" contype="1" conaffinity="1"/>
                        </body>

                      </body>

                      <body name="leftclaw" pos="0.015 0.05 0">
                        <geom name="leftclaw_it" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.0385 0.003 0.0135"  rgba="0 1 1 1.0"  />
                        <joint name="l_close" pos="0 0 0" axis="0 1 0" range= "-0.03 0" armature="100" damping="1000" limited="true"  type="slide"/>
                        <!-- <site name="leftEndEffector" pos="0.0 -0.005 0" size="0.044 0.008 0.012" type='box' /> -->
                        <!-- <site name="leftEndEffector" pos="0.035 0 0" size="0.01" rgba="1.0 0.0 0.0 1.0"/> -->
                        <site name="leftEndEffector" pos="0.0385 0 0" size="0.01" rgba="1.0 0.0 0.0 1.0"/>
                        <body name="leftpad" pos ="0 -.003 0" >
                          <geom name="leftpad_geom" condim="4" margin="0.001" type="box" user="0" pos="0 0 0" size="0.0385 0.003 0.0135" rgba="0 1 1 1.0" solimp="0.9 0.9 0.01" solref="0.01 1" friction="2 0.1 0.002"  contype="1" conaffinity="1" />
                        </body>

                      </body>
                    </body>

                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </body>
  <!--
  <body name="kuka_table" pos="0 0 1">
    <geom class="collision" type="box" size=".4 .5 .5" pos="0 0 -.5" rgba="1 1 1 1"/>
    <body name="kuka_base" pos="0.2 0 0">

    </body>
  </body>
  -->


  <body mocap="true" name="mocap" pos="0 0 0">
    <site name="mocap" pos="0 0 0" rgba="0.0 0.5 0.5 0" size="0.01" type="sphere"></site>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0.5 0.5 0.5 0.7" size="0.005 0.005 0.005" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0.5 0 0 0.1" size="1 0.005 0.005" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="0.005 1 0.005" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0 0.5 0.1" size="0.005 0.005 1" type="box"/>
  </body>

</mujoco>
