<?xml version="1.0" encoding="utf-8"?>
<mujoco>
    <include file="shared_config.xml"></include>
    <worldbody>
        <include file="sawyer_xyz_base.xml"></include>
        <body name="stick" pos="-0.15 0.75 0.02">
          <inertial pos="0 0 0" mass="1" diaginertia="100000 100000 100000"/>
          <geom rgba="1 0.3 0.3 1" pos="0 0 0" size="0.05 0.02 0.02" type="box" solimp="0.998 0.998 0.001" solref="0.02 1" conaffinity="1" contype="1" group="1"/>
          <joint type="free" limited='false' damping="0.0005"/>
        </body>
        <!-- <body name="object" pos="0 0.8 0.04">
            <joint type="slide" range="-10.3213 10.3" axis="1 0 0" name="obj_slidex" pos="0 0 0" damping="1.0"/>
            <joint type="slide" range="-10.3213 10.3" axis="0 1 0" name="obj_slidey" pos="0 0 0" damping="1.0"/>
            <geom conaffinity="1" contype="1" friction="1.0 0.1 0.02" mass="1" rgba="0 0 1 1" size="0.04 0.04 0.04" type="box"/>
            <geom type="box" pos="0.045 0 0" size="0.005 0.005 0.03" rgba="0 0 0 1" contype="1" conaffinity="1" name="handle"/>
            <geom type="box" pos="0.02 0 0.025" size="0.02 0.005 0.005" rgba="0 0 0 1" contype="1" conaffinity="1"/>
            <geom type="box" pos="0.02 0 -0.025" size="0.02 0.005 0.005" rgba="0 0 0 1" contype="1" conaffinity="1"/>
        </body> -->
        <body name="object" pos="0.2 0.6 0.08">
            <joint type="slide" range="-0.2 0.2" axis="1 0 0" name="obj_slidex" pos="0 0 0" damping="1" limited="true"/>
            <joint type="slide" range="-0.2 0.2" axis="0 1 0" name="obj_slidey" pos="0 0 0" damping="1" limited="true"/>
            <!-- <inertial mass="0.001" pos="0 0 0" diaginertia="100000 100000 100000"/> -->
            <!-- <geom conaffinity="1" contype="1" mass="0.1" rgba="0 0 1 1" size="0.04 0.04 0.06" type="box" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.095 0" size="0.005 0.005 0.04" rgba="0 0 0 1" contype="1" conaffinity="1" name="handle" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.065 0.035" size="0.005 0.025 0.005" rgba="0 0 0 1" contype="1" conaffinity="1" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.065 -0.035" size="0.005 0.025 0.005" rgba="0 0 0 1" contype="1" conaffinity="1" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <site name="insertion" pos="0 -0.06 0" size="0.001" rgba="1 0 0 1"/> -->
            <geom conaffinity="1" contype="1" mass="0.1" rgba="0 0 1 1" size="0.05 0.05 0.08" type="box" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.135 0" size="0.005 0.005 0.06" rgba="0 0 0 1" contype="1" conaffinity="1" name="handle" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.09 0.055" size="0.005 0.04 0.005" rgba="0 0 0 1" contype="1" conaffinity="1" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <geom type="box" pos="0 -0.09 -0.055" size="0.005 0.04 0.005" rgba="0 0 0 1" contype="1" conaffinity="1" solimp="0.99 0.99 0.01" solref="0.01 1"/>
            <site name="insertion" pos="0 -0.09 0" size="0.001" rgba="1 0 0 1"/>
        </body>
        <site name="goal" pos="0.35 0.75 0.02" size="0.02" 
              rgba="0 0.8 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>
