<?xml version="1.0" encoding="utf-8"?>
<mujoco>
    <include file="shared_config.xml"></include>
    <worldbody>
        <include file="sawyer_xyz_base.xml"></include>

        <!-- <body name="peg" pos="0 0.6 0.05">
          <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/>
          <geom name="peg" fromto="0 0 -0.05 0 0 0.05" rgba="0.3 1 0.3 1" size="0.02 0.1" type="cylinder" conaffinity="1" contype="1" group="1"/>
          <joint type="free" limited="false" damping="0.005"/>
          <site name="pegHead" pos="0 0 -0.05" size="0.005" rgba="0.8 0 0 1"/>
        </body> -->

        <body name="peg" pos="0 0.6 0.03">
          <!-- <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/> -->
          <inertial pos="0 0 0" mass="0.75" diaginertia="8.80012e-04 8.80012e-04 8.80012e-04" />
          <geom name="peg" fromto="0 0 -0.03 0 0 0.03" rgba="0.3 1 0.3 1" size="0.03 0.035" type="cylinder" conaffinity="1" contype="1" group="1"/>
          <joint type="free" limited="false"  damping="0." armature="0."/>
          <site name="pegHead" pos="0 0 -0.03" size="0.005" rgba="0.8 0 0 1"/>
        </body>

        <!-- <body name="box" pos="0 0.8 0.02">
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.02" name="box_left" conaffinity="1" pos="0 0 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.02" name="box_right" conaffinity="1" pos="0 0.1 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.02" name="box_front" conaffinity="1" pos="0.07 0.05 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.02" name="box_behind" conaffinity="1" pos="-0.07 0.05 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <site name="hole" pos="0 0.05 0.02" size="0.005" rgba="0 0.8 0 1"/>
        </body> -->
        <body name="box" pos="0 0.8 0.015">
          <!-- <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.02" name="box_left" conaffinity="1" pos="0 0 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.02" name="box_right" conaffinity="1" pos="0 0.1 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.02" name="box_front" conaffinity="1" pos="0.07 0.05 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.02" name="box_behind" conaffinity="1" pos="-0.07 0.05 0" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/> -->
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.015" name="box_left" conaffinity="1" pos="0 0 0" mass="1000"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.01 0.015" name="box_right" conaffinity="1" pos="0 0.1 0" mass="1000"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.015" name="box_front" conaffinity="1" pos="0.07 0.05 0" mass="1000"/>
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.03 0.04 0.015" name="box_behind" conaffinity="1" pos="-0.07 0.05 0" mass="1000"/>
          <site name="hole" pos="0 0.05 0.015" size="0.005" rgba="0 0.8 0 1"/>
        </body>
        <site name="goal" pos="0 0.8 0.1" size="0.01" rgba="0.8 0 0 1"/>
    </worldbody>

    <actuator>
        <position ctrllimited="true" ctrlrange="-1 1" joint="r_close" kp="400"  user="1"/>
        <position ctrllimited="true" ctrlrange="-1 1" joint="l_close" kp="400"  user="1"/>
    </actuator>

</mujoco>
