<?xml version="1.0" encoding="utf-8"?>
<mujoco>
    <compiler meshdir="meshes" texturedir="textures"/>
    <include file="include/assets.xml"/>
    <include file="include/actuators_torqueCtrl.xml"/>
    <option timestep='0.0025'/>


    <worldbody>
        <body name="sawyer">
            <include file="include/sawyer_chain.xml"/>
        </body>

        <include file="include/table.xml"/>

        <camera name="track" pos="0 -0.4 1.3" mode="fixed" euler="0.85 -0 0"/>
        <camera name="track_aux_shelf" pos="1.6 -0.4 1.7" mode="fixed" euler="0.95 0 1"/>
        <camera name="track_aux_shelf2" pos="1.4 0.4 0.6" mode="fixed" euler="1.3 0 1.57"/>

        <body name="mug0" pos="0 0.6 0.007">
            <joint name="mugjoint0" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="23.78" diaginertia=".1 .1 .1"/>
            <geom name="mugGeom0"
                  mesh="mug_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="wood"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
            <site name="mugSite0" pos="0 0 0" size="0.01" rgba="0 0 0 0"/>
            <site name="mugSite0_top" pos="0 0 0" size="0.01" rgba="0 0 0 0"/>

        </body>


        <body name="shelf" pos="0. 1.05 0.001">
            <geom type="box" contype="1" material="light_wood" size="0.6 0.2 0.001" name="level1" conaffinity="1"
                  pos="0 0 0" mass="1000"/>
            <geom type="box" contype="1" material="light_wood" size="0.6 0.2 0.001" name="level2" conaffinity="1"
                  pos="0 0 0.1" mass="1000"/>
            <geom type="box" contype="1" material="light_wood" size="0.6 0.2 0.001" name="cover" conaffinity="1"
                  pos="0 0 0.6" mass="1000"/>
            <geom type="box" contype="1" material="light_wood" size="0.6 0.001 0.3" name="wall1" conaffinity="1"
                  pos="0.0 0.2 0.301" mass="1000"/>
            <!-- <geom rgba="0.3 0.3 1 1" type="box" solimp="0.99 0.99 0.01" contype="1" size="0.1 0.001 0.03" name="cover_wall2" conaffinity="1" pos="0.0 -0.1 0.034" mass="1000" solref="0.01 1"/> -->
            <geom material="light_wood" type="box" contype="1" size="0.001 0.2 0.3" name="wall3" conaffinity="1"
                  pos="0.6 0 0.301" mass="1000"/>
            <geom material="light_wood" type="box" contype="1" size="0.001 0.2 0.3" name="wall4" conaffinity="1"
                  pos="-0.6 0 0.301" mass="1000"/>

            <site name="goal" pos="0 -0.1 0.105" size="0.02" rgba="0 0.8 0 0"/>
            <site name="goal_top" pos="0 -0.1 0.205" size="0.02" rgba="0 0.8 0 0"/>
            <!-- <joint type="slide" range="-0.2 0." axis="0 1 0" name="goal_slidey" pos="0 0 0" damping="1.0"/> -->
        </body>

    </worldbody>
    <!-- <sensor>
        <touch name='rightclaw_sensor' site='rightEndEffector'/>
        <touch name='leftclaw_sensor' site='leftEndEffector'/>
    </sensor> -->
    <!--    <actuator>-->
    <!--        <position name="j0" joint="right_j0" forcerange="-10 10" ctrlrange="-3 3" kp="50"/>-->
    <!--        <position name="j1" joint="right_j1" forcerange="-10 10" ctrlrange="-3.5 2" kp="50"/>-->
    <!--        <position name="j2" joint="right_j2" forcerange="-10 10" ctrlrange="-3 3" kp="50"/>-->
    <!--        <position name="j3" joint="right_j3" forcerange="-10 10" ctrlrange="-3 3" kp="50"/>-->
    <!--        <position name="j4" joint="right_j4" forcerange="-10 10" ctrlrange="-2.9 2.9" kp="50"/>-->
    <!--        <position name="j5" joint="right_j5" forcerange="-10 10" ctrlrange="-2.9 2.9" kp="50"/>-->
    <!--        <position name="j6" joint="right_j6" forcerange="-10 10" ctrlrange="-4.7 4.7" kp="50"/>-->
    <!--        <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>-->

    <keyframe>
        <key qpos='-0.029997 -0.56 0.0299998 2.10036 0.11904 -1.16064 -1.46072 0.0092 -0.00705 -0.125 0.7 0 1 0 0 0'/>
    </keyframe>

</mujoco>
