<mujoco>
  <compiler angle="radian" meshdir="meshes/" />
  <asset>
    <include file="shared_assets.xml"></include>
  </asset>
  <default>
    <include file="shared.xml"></include>
  </default>
  <equality>
    <weld body1="mocap" body2="right_gripper_base" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
  </equality>
	<option timestep="0.002">
		<flag warmstart="enable"></flag>
	</option>

  <worldbody>
    <light directional="true" ambient="0.2 0.2 0.2" diffuse="0.8 0.8 0.8" specular="0.03 0.03 0.03" castshadow="false" pos="2 2 2" dir="-1 -.2 -.8" name="light0"></light>
    <geom name="floor0" pos="0.4 0 -0.8" size="2.2 2.7 1" type="plane" condim="3" material="floor_mat"></geom>

    <camera name="camera_topdown" mode="fixed" zaxis="0 0 1" pos="0 0 3" />
    <camera name="camera_side" mode="fixed" zaxis="0 -1 0" pos="0.4 -2 0.5" />
    <camera name="camera_front" mode="fixed" xyaxes="0 1 0 0 0 1" pos="2.4 0 0.5" />

    <body name="object0" pos="0.75 0.25 -0.14">
      <joint name="object0:joint" type="free" damping="0.01"/>
      <geom size="0.02 0.02 0.02" type="box" condim="3" name="object0" contype="1" conaffinity="1"
            material="block_mat" mass="2"/>
      <site name="object0" pos="0 0 0" size="0.01 0.01 0.01" rgba="1 0 0 1" type="sphere"/>
    </body>
    <include file="sawyer_robot.xml"/>
    <body mocap="true" name="mocap" pos="0.2369680365278436 0.7377722688452103 0.30233579064225752">
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1.0 1.0 1.0 0.0" size="0.02 0.02 0.02" type="box"/>
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="1 0.005 0.005" type="box"/>
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="0.005 1 0.001" type="box"/>
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="0.005 0.005 1" type="box"/>
      <!--<geom conaffinity="0" contype="0" pos="0 0 .1" rgba="1 0 0 1" size="0.005" type="sphere" name="gripper_pos"/>-->
    </body>

    <body name="task_marker" pos="0.65 0 0">
      <geom size="0.12 0.01" pos="0 -.3 0" type="cylinder" rgba="0.9 0.4 0.4 1" name="target1" />
      <geom size="0.12 0.01" pos="0 0 0" type="cylinder" rgba="0.4 0.9 0.4 1" name="target2" />
      <geom size="0.12 0.01" pos="0 .3 0" type="cylinder" rgba="0.4 0.4 0.9 1" name="target3" />
    </body>

    <body name="achieved_goal">
      <joint name="achieved_goal:joint" type="free" damping="0.01"/>
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="1 0 0 1" size="0.01" type="sphere" name="achieved_goal"/>
    </body>

    <body name="desired_goal">
      <joint name="desired_goal:joint" type="free" damping="0.01"/>
      <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 1 0 1" size="0.01" type="sphere" name="desired_goal"/>
    </body>
  </worldbody>
S
  <actuator>
    <!-- Finger joints are position-controlled -->
    <!--<motor gear="100" joint="r_gripper_l_finger_joint" name="slide_l_claw"/>-->
    <!--<motor gear="100" joint="r_gripper_r_finger_joint" name="slide_r_claw"/>-->

    <position ctrllimited="true" ctrlrange="0 0.020833" joint="r_gripper_l_finger_joint" kp="30000" name="slide_l_claw"
              user="1"/>
    <position ctrllimited="true" ctrlrange="-0.020833 0" joint="r_gripper_r_finger_joint" kp="30000" name="slide_r_claw"
              user="1"/>

    <!--<motor joint="right_j0" ctrlrange="-100.0 100.0" ctrllimited="false" gear="150"/>-->
    <!--<motor joint="right_j1" ctrlrange="-100.0 100.0" ctrllimited="false" gear="100"/>-->
    <!--<motor joint="right_j2" ctrlrange="-100.0 100.0" ctrllimited="false" gear="120"/>-->
    <!--<motor joint="right_j3" ctrlrange="-100.0 100.0" ctrllimited="false" gear="150"/>-->
    <!--<motor joint="right_j4" ctrlrange="-100.0 100.0" ctrllimited="false" gear="150"/>-->
    <!--<motor joint="right_j5" ctrlrange="-100.0 100.0" ctrllimited="false" gear="150"/>-->
    <!--<motor joint="right_j6" ctrlrange="-100.0 100.0" ctrllimited="false" gear="150"/>-->
  </actuator>

</mujoco>
