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


    <worldbody>
        <light diffuse="0.3 0.3 0.3" pos="0 -1 3" dir="0 0.5 -1"/> 


        <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="100" diaginertia=".1 .1 .1"/>
            <geom name="mugGeom0"
                  mesh="cup_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="0 0 1.58"
                  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_bowl1" pos="-0.44 1 0.042">
            <joint name="shelf_bowl1" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom1"
                  mesh="bowl1_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="blue"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="0.8 0.8 0.8 0.4"/>
        </body>



        <body name="shelf_bowl2" pos="-0.23 0.99 0.042">
            <joint name="shelf_bowl2" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom2"
                  mesh="bowl2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
        </body>





        <body name="shelf_bowl3" pos="0.1 1.0 0.042">
            <joint name="shelf_bowl3" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom3"
                  mesh="bowl3_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="wood_material"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="0.6 0.6 0.6 1"/>
        </body>



        <body name="shelf_plate" pos="0.35 1.0 0.04">
            <joint name="shelf_plate" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_plate_geom"
                  mesh="plate_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>


        <body name="shelf_plate_1" pos="0.35 1.0 0.06">
            <joint name="shelf_plate_1" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_plate_geom_1"
                  mesh="plate_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>


        <body name="shelf_plate_2" pos="0.35 1.0 0.08">
            <joint name="shelf_plate_2" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_plate_geom_2"
                  mesh="plate_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>

        <body name="shelf_plate_3" pos="0.35 1.0 0.10">
            <joint name="shelf_plate_3" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_plate_geom_3"
                  mesh="plate_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>

        <body name="shelf_plate_4" pos="0.35 1.0 0.12">
            <joint name="shelf_plate_4" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_plate_geom_4"
                  mesh="plate_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>













        <body name="shelf_bowl4" pos="0.26 1.2 0.275">
            <joint name="shelf_bowl4" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom4"
                  mesh="bowl4_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
        </body>
        
<!--         <body name="shelf_bowl41" pos="-0.34 1.13 0.31">
            <joint name="shelf_bowl41" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom41"
                  mesh="bowl4_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
        </body> -->
<!-- 
        <body name="shelf_bowl42" pos="0.29 1.08 0.31">
            <joint name="shelf_bowl42" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bowl_geom42"
                  mesh="bowl4_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="metal"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
        </body> -->



        <body name="shelf_cup2" pos="0.49 1.22 0.275">
            <joint name="shelf_cup2" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_cup_geom2"
                  mesh="cup2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 0"
                  rgba="1 1 1 1"/>
        </body>


        <body name="shelf_cup21" pos="0.505 1.12 0.275">
            <joint name="shelf_cup21" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_cup_geom21"
                  mesh="cup2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 3"
                  rgba="1 1 1 1"/>
        </body>


        <body name="shelf_cup22" pos="0.39 1.18 0.275">
            <joint name="shelf_cup22" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_cup_geom22"
                  mesh="cup2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 1"
                  rgba="1 1 1 1"/>
        </body>



        <body name="shelf_cup23" pos="0.5 1.0 0.275">
            <joint name="shelf_cup23" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_cup_geom23"
                  mesh="cup2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 -1"
                  rgba="1 1 1 1"/>
        </body>



        <body name="shelf_cup24" pos="0.4 1.05 0.275">
            <joint name="shelf_cup24" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_cup_geom24"
                  mesh="cup2_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="white"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  euler="1.58 0 -2"
                  rgba="1 1 1 1"/>
        </body>



        <body name="shelf_can1" pos="-0.526 1.2 0.275">
            <joint name="shelf_can1" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_can_geom1"
                  mesh="can_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="red"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="0.8 0.8 0.8 1"/>
        </body>


        <body name="shelf_can2" pos="-0.53 0.9 0.275">
            <joint name="shelf_can2" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_can_geom2"
                  mesh="can_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="blue"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="0.8 0.8 0.8 1"/>
        </body>


        <body name="shelf_box1" pos="-0.5 1.0 0.275">
            <joint name="shelf_box1" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_box_geom1"
                  mesh="box_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 0.8 0.8"/>
        </body>


        <body name="shelf_box2" pos="-0.45 1.2 0.275">
            <joint name="shelf_box2" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_box_geom2"
                  mesh="box_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="0.8 0.8 0.8 1"/>
        </body>


        <body name="shelf_bottle" pos="-0.44 1.09 0.275">
            <joint name="shelf_bottle" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bottle_geom"
                  mesh="bottle_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="grey"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="1 1 1 1"/>
        </body>


        <body name="shelf_bottle1" pos="-0.52 1.1 0.275">
            <joint name="shelf_bottle1" type="free" limited='false' damping="1" armature="0"/>
            <inertial pos="0 0 0" mass="100" diaginertia=".1 .1 .1"/>
            <geom name="shelf_bottle_geom1"
                  mesh="bottle_mesh"
                  conaffinity="1"
                  contype="1"
                  friction=".1 .005 .0001"
                  density="1384.7"
                  type="mesh"
                  material="grey"
                  solimp="0.99 0.99 0.01"
                  solref="0.01 1"
                  rgba="0.8 0.8 0.8 1"/>
        </body>




        <body name="shelf" pos="0. 1.05 0.001">
            <geom type="box" contype="1" material="light_wood" size="0.6 0.2 0.007" name="level1" conaffinity="1"
                  pos="0 0 0.03" mass="1000"/>
            <geom type="box" contype="1" material="light_wood" size="0.6 0.2 0.007" name="level2" conaffinity="1"
                  pos="0 0 0.2" mass="1000"/>
            <geom type="box" contype="1" material="light_wood" size="0.61 0.2 0.007" name="cover" conaffinity="1"
                  pos="0 0 0.6" mass="1000"/>
<!--             <geom type="box" contype="1" material="light_wood" size="0.6 0.005 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.005 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.005 0.2 0.3" name="wall4" conaffinity="1"
                  pos="-0.6 0 0.301" mass="1000"/>
            <geom material="light_wood" type="box" contype="1" size="0.005 0.2 0.3" name="wall3_inner" conaffinity="1"
                  pos="0.575 0 0.301" mass="1000"/>
            <geom material="light_wood" type="box" contype="1" size="0.005 0.2 0.3" name="wall4_inner" conaffinity="1"
                  pos="-0.575 0 0.301" mass="1000"/>


            <site name="goal" pos="0.2 -0.1 0.27" size="0.02" rgba="0 0.8 0 1"/>
            <site name="goal_top" pos="0.2 -0.1 0.37" size="0.02" rgba="0 0.8 0 1"/>
            <!-- <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'/> -->
<!--         <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 -0.44 1 0.042 1 0 0 0 -0.23 0.99 0.042 1 0 0 0 0.1 1 0.042 1 0 0 0 0.35 1 0.04 1 0 0 0 0.35 1 0.06 1 0 0 0 0.35 1 0.08 1 0 0 0 0.35 1 0.1 1 0 0 0 0.35 1 0.12 1 0 0 0 0.26 1.2 0.31 1 0 0 0 0.49 1.22 0.275 1 0 0 0 0.505 1.12 0.275 1 0 0 0 0.39 1.18 0.275 1 0 0 0 0.5 1 0.275 1 0 0 0 0.4 1.05 0.275 1 0 0 0 -0.526 1.2 0.275 1 0 0 0 -0.53 0.9 0.275 1 0 0 0 -0.5 1 0.275 1 0 0 0 -0.45 1.2 0.275 1 0 0 0 -0.44 1.09 0.275 1 0 0 0 -0.52 1.1 0.275 1 0 0 0'/> -->
        <key qpos='-0.061 -0.59 0.0299998 2.19168 0.11904 -1.30944 -1.46072 0.002 0 -0.125 0.7 0 1 0 0 0 -0.44 1 0.042 1 0 0 0 -0.23 0.99 0.042 1 0 0 0 0.1 1 0.042 1 0 0 0 0.35 1 0.04 1 0 0 0 0.35 1 0.06 1 0 0 0 0.35 1 0.08 1 0 0 0 0.35 1 0.1 1 0 0 0 0.35 1 0.12 1 0 0 0 0.26 1.2 0.275 1 0 0 0 0.49 1.22 0.275 1 0 0 0 0.505 1.12 0.275 1 0 0 0 0.39 1.18 0.275 1 0 0 0 0.5 1 0.275 1 0 0 0 0.4 1.05 0.275 1 0 0 0 -0.526 1.2 0.275 1 0 0 0 -0.53 0.9 0.275 1 0 0 0 -0.5 1 0.275 1 0 0 0 -0.45 1.2 0.275 1 0 0 0 -0.44 1.09 0.275 1 0 0 0 -0.52 1.1 0.275 1 0 0 0'/>
    </keyframe>

</mujoco>
