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

        <body name="faucetBase" pos="0 0.8 .05">
        <geom name="base" rgba="0.3 1 0.3 1" size=".03 .03 .05" type="box" margin="0.001"
                pos="0 0 0" contype="1" conaffinity="1" condim="3" friction="1 0.1 0.002"
            />
        </body>

        <body name="faucet" pos="0 0.8 .05">
          <!-- <inertial pos="0 0 0" mass="1" diaginertia="8.80012e-4 8.80012e-4 8.80012e-4"/> -->
          <geom name="handle" rgba="0.3 1 0.3 1" size=".015 .05 .015" type="box" margin="0.001"
                pos="0 -0.05 0.065" contype="1" conaffinity="1" condim="3" friction="1 0.1 0.002"
            />
          <joint type="hinge" range="-1.5708 1.5708" axis="0 0 1" name="joint" pos="0 0 0.065" damping="1" armature="0" limited="true"/>
          <site name="handleStartOpen" pos="-0.015 -0.1 0.065" size="0.005" rgba="1 1 1 1"/>
          <!-- <site name="handleStartClose" pos="0.015 -0.1 0.065" size="0.005" rgba="0 0 0 1"/> -->
          <site name="handleStartClose" pos="0.015 -0.1 0.065" size="0.005" rgba="0 0 0 1"/>
        </body>

        <site name="goal_open" pos="0.1 0.785 0.115" size="0.005"
              rgba="0 .8 0 1"/>
        <site name="goal_close" pos="-0.1 0.785 0.115" size="0.005"
              rgba="0 0 0.8 1"/>

<!--         <body name="box" pos="0 0.8 0.05">
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.1 0.05 0.05" name="box_left" 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.05 0.05" name="box_right" conaffinity="1" pos="0 0.16 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.035 0.03 0.05" name="box_front" conaffinity="1" pos="0.065 0.08 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.035 0.03 0.05" name="box_behind" conaffinity="1" pos="-0.065 0.08 0" 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"/>
        </body>
 -->    </worldbody>

    <actuator>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j0" name="torq_right_j0"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j1" name="torq_right_j1"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j2" name="torq_right_j2"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j3" name="torq_right_j3"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j4" name="torq_right_j4"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j5" name="torq_right_j5"/>
      <motor ctrllimited="false" ctrlrange="-500 500" joint="right_j6" name="torq_right_j6"/>
      <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>
