<?xml version="1.0" encoding="utf-8"?>
<mujoco>
    <!-- update config -->
    <include file="shared_config_kuka.xml"></include>
    <worldbody>
        <!-- update base -->
        <include file="sawyer_xyz_base_kuka.xml"></include>

        <!-- todo: update position -->
        <body name="obj" pos="0.6 0 0.02">
            <joint name="objjoint" type="free" limited='false' damping="0.0" armature="0" />
            <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/>
            <geom name="objGeom" type="box" pos="0 0 0" solimp="0.99 0.99 0.01" size="0.02 0.02 0.02" rgba="0 0 1 1" solref="0.01 1" contype="1" conaffinity="1" friction="0.1 1 0.002" condim="4" material="wood" />
            <!-- <geom name="objGeom" type="box" pos="0 0 0" solimp="0.99 0.99 0.01" size="0.02 0.02 0.02" rgba="0 0 1 1" solref="0.01 1" contype="1" conaffinity="1" friction="2 0.1 0.002" condim="4" /> -->
            <!--  <site name="obj" pos="0 0 0" size="0.01" /> -->
        </body>

        <!-- todo: update position -->
        <body name="shelf" pos="0.85 0. 0.001">
            <geom rgba="1 0.3 0.3 1" type="box" contype="1" size="0.1 0.1 0.001" name="level1" conaffinity="1" pos="0 0 0" mass="1000" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="box" contype="1" size="0.1 0.1 0.001" name="level2" conaffinity="1" pos="0 0 0.2" mass="1000" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="box" contype="1" size="0.1 0.1 0.001" name="cover" conaffinity="1" pos="0 0 0.6" mass="1000" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="box" contype="1" size="0.001 0.1 0.3" name="wall1" conaffinity="1" pos="0.1 0.0 0.301" mass="1000" material="light_wood"/>
            <!-- <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 rgba="1 0.3 0.3 1" type="box" contype="1" size="0.1 0.001 0.3" name="wall3" conaffinity="1" pos="0 0.1 0.301" mass="1000" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="box" contype="1" size="0.1 0.001 0.3" name="wall4" conaffinity="1" pos="0 -0.101 0.301" mass="1000" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="capsule" fromto="0.103 0.103 0.0075 0.103 0.103 0.6" contype="2" size="0.005" conaffinity="2" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="capsule" fromto="-0.103 0.103 0.0075 -0.103 0.103 0.6" contype="2" size="0.005" conaffinity="2" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="capsule" fromto="0.103 -0.103 0.0075 0.103 -0.103 0.6" contype="2" size="0.005" conaffinity="2" material="light_wood"/>
            <geom rgba="1 0.3 0.3 1" type="capsule" fromto="-0.103 -0.103 0.0075 -0.103 -0.103 0.6" contype="2" size="0.005" conaffinity="2" material="light_wood"/>
            <site name="goal" pos="0 0 0.3" 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 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>
