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

        <body name="obj" pos="0 0.6 0.02">
            <joint name="objjoint" type="free" limited='false' damping="1" armature="0" />
            <inertial pos="0 0 0" mass=".1" diaginertia=".1 .1 .1"/>
                <geom name="objGeom" 
                  mesh="mug_mesh" 
                  conaffinity="1" 
                  contype="1" 
                  friction=".1 .005 .0001"
                  density="1384.7" 
                  type="mesh" 
                  material="navy_blue"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>

            <site name="objSite" pos="0 0 0" size="0.01" rgba="0 0 0 0"/>
        </body>

        <body name="button" pos="0 0.78 0.33">
          <inertial pos="0 0 0" mass="0.1" diaginertia="100000 100000 100000"/>
          <geom name="button" fromto="0 -0.03 0 0 0.03 0" rgba="0.3 1 0.3 1" size="0.03 0.06" type="cylinder" solref="0.02 1" conaffinity="1" contype="1" group="1"/>
          <joint type="slide" range="0 0.05" axis="0 1 0" name="goal_slidey" pos="0 0 0" damping="1.0"/>
          <site name="buttonStart" pos="0 -0.03 0" size="0.005" rgba="0 0.8 0 1"/>
        </body>

        <body name="coffee_machine" pos="0 0.9 0.28">
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.1 0.02" name="box_bottom" 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.1 0.02" name="box_top" conaffinity="1" pos="0 0 0.1" 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.035 0.1 0.03" name="box_left" conaffinity="1" pos="0.065 0 0.05" 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.035 0.1 0.03" name="box_right" conaffinity="1" pos="-0.065 0 0.05" 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.05 0.13" name="expresso_base" conaffinity="1" pos="0 0.05 -0.15" mass="1000" solimp="0.99 0.99 0.01" solref="0.01 1"/>
          <!-- <joint type="slide" range="-0.2 0." axis="0 1 0" name="goal_slidey" pos="0 0 0" damping="1.0"/> -->
          <site name="coffee_goal" pos="0 -0.1 -0.27" size="0.01" rgba="1 0 0 1"/>
        </body>
        <site name="mug_goal" pos="0 0.6 0.02" size="0.01" rgba="0 1 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>
