<?xml version="1.0" encoding="utf-8"?>
<mujoco>
    <include file="shared_config.xml"/>
    <worldbody>
        <include file="sawyer_xyz_base.xml"/>
        <body name="door_0" pos="0.30 0.8 0.2">
            <joint name="door_0_joint" pos="-0.1 0 0" axis="0 0 1" type="hinge" limited="true" range="-1.507 0"
                   damping="10" armature="0"/>
            <geom type="box" pos="0 0 0" size="0.1 0.005 0.2" rgba="0 0.5 1 1" contype="1" conaffinity="1"/>
            <geom type="box" pos="0.05 -0.04 0" size="0.05 0.005 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <geom type="box" pos="0.095 -0.025 0" size="0.005 0.01975 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <site name="door_0_handle" pos="0.05 -0.04 0" size="0.01" rgba="1 0 0 1"/>
        </body>
        <body name="door_1" pos="0 0.8 0.2">
            <joint name="door_1_joint" pos="-0.1 0 0" axis="0 0 1" type="hinge" limited="true" range="-1.507 0"
                   damping="10" armature="0"/>
            <geom type="box" pos="0 0 0" size="0.1 0.005 0.2" rgba="1 0.5 0.5 1" contype="1" conaffinity="1"/>
            <geom type="box" pos="0.05 -0.04 0" size="0.05 0.005 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <geom type="box" pos="0.095 -0.025 0" size="0.005 0.01975 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <site name="door_1_handle" pos="0.05 -0.04 0" size="0.01" rgba="1 0 0 1"/>
        </body>
        <body name="door_2" pos="-0.3 0.8 0.2">
            <joint name="door_2_joint" pos="-0.1 0 0" axis="0 0 1" type="hinge" limited="true" range="-1.507 0"
                   damping="10" armature="0"/>
            <geom type="box" pos="0 0 0" size="0.1 0.005 0.2" rgba="0.4 0.8 0.4 0.8" contype="1" conaffinity="1"/>
            <geom type="box" pos="0.05 -0.04 0" size="0.05 0.005 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <geom type="box" pos="0.095 -0.025 0" size="0.005 0.01975 0.005" rgba=".75 .75 .75 1" contype="1"
                  conaffinity="1"/>
            <site name="door_2_handle" pos="0.05 -0.04 0" size="0.01" rgba="1 0 0 1"/>
        </body>
        <body name="obj" pos="0 0.6 0.02">
            <joint name="objjoint" type="free" limited='false' damping="0" armature="0"/>
            <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/>
            <geom name="objGeom" type="box" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 0.5 1 1" contype="1"
                  conaffinity="1" friction="2 0.1 0.002" condim="4"/>
            <site name="obj" pos="0 0 0" size="0.01"/>
        </body>
        <body name="distractor_1" pos="-0.1 0.6 0.02">
            <joint name="distractor_1" type="free" limited='false' damping="0" armature="0"/>
            <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/>
            <geom name="distractor_1" type="box" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0.5 0.5 0.8" contype="1"
                  conaffinity="1" friction="2 0.1 0.002" condim="4"/>
        </body>
        <body name="distractor_2" pos="-0.1 0.6 0.02">
            <joint name="distractor_2" type="free" limited='false' damping="0" armature="0"/>
            <inertial pos="0 0 0" mass="0.01" diaginertia="100000 100000 100000"/>
            <geom name="distractor_2" type="box" pos="0 0 0" size="0.02 0.02 0.02" rgba="0.4 0.8 0.4 0.8" contype="1"
                  conaffinity="1" friction="2 0.1 0.002" condim="4"/>
        </body>
        <site name="hand-goal-site" pos="0 0 0" size="0.01" rgba="1 0 0 0.3"/>
        <site name="obj-goal-site" pos="0 0 0" size="0.01" rgba="0 0.5 1 1"/>
        <light directional="true" ambient="0.2 0.2 0.2" diffuse="0.8 0.8 0.8" specular="0.3 0.3 0.3" castshadow="false"
               pos="-0.4 0.2 4" dir="0.1 0.2 -0.1" name="light0"/>
        <light directional="true" ambient="0.02 0.02 0.02" diffuse="0.8 0.8 0.8" specular="0.3 0.3 0.3"
               castshadow="false"
               pos="4 4 1" dir="-0.2 0 -0.25" name="light1"/>
    </worldbody>
    <sensor>
        <touch name='rightclaw_sensor' site='rightEndEffector'/>
        <touch name='leftclaw_sensor' site='leftEndEffector'/>
    </sensor>
    <actuator>
        <motor ctrllimited="true" ctrlrange="-1 1" gear="200" joint="r_close"/>
        <motor ctrllimited="true" ctrlrange="-1 1" gear="200" joint="l_close"/>
    </actuator>
</mujoco>
