<?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.02">

            <joint name="objjoint" type="free" limited='false' damping="0." armature="0."/>

            <inertial pos="0 0 0" mass=".75" diaginertia="8.80012e-04 8.80012e-04 8.80012e-04"/>
            <geom name="objGeom" type="cylinder" pos="0 0 0" solimp="0.99 0.99 0.01"
                  size="0.03 0.015" rgba="1 0 0 1" solref="0.01 1"
                  contype="1" conaffinity="1" friction="1 0.1 0.002" condim="4" material="wood"
            />
        </body>

        <!-- reach goal -->
        <site name="goal_reach" pos="-0.1 0.8 0.2" size="0.02"
              rgba="0.8 0 0 1"/>
        <!-- push goal -->
        <site name="goal_push" pos="0.1 0.8 0.02" size="0.02"
              rgba="0 0.8 0 1"/>
        <!-- place goal -->
        <site name="goal_pick_place" pos="0.1 0.8 0.2" size="0.02"
              rgba="0 0 0.8 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>
