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

        <body name="obj" pos="0 0.6 0.03">
            <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="sphere" pos="0 0 0" solimp="0.99 0.99 0.01"
                  size="0.03" rgba="0 0 0 1" solref="0.01 1"
                  contype="1" conaffinity="1" friction="1 0.1 0.002" condim="4"
            />
            <!-- <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>

         <body name="goal_whole" pos="0. 0.9 0.02">
          <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.105 0.04 0.001" name="goal_top" conaffinity="1" pos="0 0 0.2" mass="1000"/>
          <!-- <geom rgba="0.3 0.3 1 1" type="box" contype="1" size="0.12 0.12 0.001" name="goal_bottom" conaffinity="1" pos="0 0 0.005" mass="1000"/> -->
          <!-- <geom rgba="0.3 0.3 1 1" type="box" solimp="0.99 0.99 0.01" contype="1" size="0.105 0.001 0.1" name="goal_wall1" conaffinity="1" pos="0.0 0.105 0.102" mass="1000" solref="0.01 1"/> -->
          <geom rgba="0.3 0.3 1 1" type="box" solimp="0.99 0.99 0.01" contype="1" size="0.105 0.001 0.1" name="goal_wall1" conaffinity="1" pos="0.0 0.041 0.102" mass="1000" solref="0.01 1"/>
          <!-- <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="goal_wall2" conaffinity="1" pos="0.0 -0.1 0.034" mass="1000" solref="0.01 1"/> -->
          <geom rgba="0.3 0.3 1 1" type="box" solimp="0.99 0.99 0.01" contype="1" size="0.001 0.04 0.1" name="goal_wall3" conaffinity="1" pos="0.105 0 0.102" mass="1000" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="box" solimp="0.99 0.99 0.01" contype="1" size="0.001 0.04 0.1" name="goal_wall4" conaffinity="1" pos="-0.106 0 0.102" mass="1000" solref="0.01 1"/>
          <geom rgba="0.3 0.3 1 1" type="capsule" fromto="0.103 0.042 0.0075 0.103 0.042 0.2" contype="2" size="0.005" conaffinity="2"/>
          <geom rgba="0.3 0.3 1 1" type="capsule" fromto="0.103 -0.042 0.0075 0.103 -0.042 0.2" contype="2" size="0.005" conaffinity="2"/>
          <geom rgba="0.3 0.3 1 1" type="capsule" fromto="-0.103 0.042 0.0075 -0.103 0.042 0.2" contype="2" size="0.005" conaffinity="2"/>
          <geom rgba="0.3 0.3 1 1" type="capsule" fromto="-0.103 -0.042 0.0075 -0.103 -0.042 0.2" contype="2" size="0.005" conaffinity="2"/>
          <!-- <joint type="slide" range="-0.2 0." axis="0 1 0" name="goal_slidey" pos="0 0 0" damping="1.0"/> -->
        </body>

        <site name="goal" pos="0. 0.9 0.02" size="0.02"
              rgba="0 0.8 0 1"/>

  </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>
