<mujoco>
    <include file="scene.xml"/>
    <include file="vx300s_dependencies.xml"/>

    <equality>
        <weld body1="mocap_left" body2="vx300s_left/gripper_link" solref="0.01 1" solimp=".25 .25 0.001" />
        <weld body1="mocap_right" body2="vx300s_right/gripper_link" solref="0.01 1" solimp=".25 .25 0.001" />
    </equality>


    <worldbody>
        <include file="vx300s_left.xml" />
        <include file="vx300s_right.xml" />

        <body mocap="true" name="mocap_left" pos="0.095 0.50 0.425">
            <site pos="0 0 0" size="0.003 0.003 0.03" type="box" name="mocap_left_site1" rgba="0 0 1 1"/>
            <site pos="0 0 0" size="0.003 0.03 0.003" type="box" name="mocap_left_site2" rgba="0 0 1 1"/>
            <site pos="0 0 0" size="0.03 0.003 0.003" type="box" name="mocap_left_site3" rgba="0 0 1 1"/>
        </body>
        <body mocap="true" name="mocap_right" pos="-0.095 0.50 0.425">
            <site pos="0 0 0" size="0.003 0.003 0.03" type="box" name="mocap_right_site1" rgba="1 0 0 1"/>
            <site pos="0 0 0" size="0.003 0.03 0.003" type="box" name="mocap_right_site2" rgba="1 0 0 1"/>
            <site pos="0 0 0" size="0.03 0.003 0.003" type="box" name="mocap_right_site3" rgba="1 0 0 1"/>
        </body>

        <body name="box" pos="0.2 0.5 0.05">
            <joint name="red_box_joint" type="free" frictionloss="0.01" />
            <inertial pos="0 0 0" mass="0.05" diaginertia="0.002 0.002 0.002" />
            <geom condim="4" solimp="2 1 0.01" solref="0.01 1" friction="1 0.005 0.0001" pos="0 0 0" size="0.02 0.02 0.02" type="box" name="red_box" rgba="1 0 0 1" />
        </body>

    </worldbody>

    <actuator>
        <position name="left_l_finger_t" ctrllimited="true" ctrlrange="-0.01 0.057" joint="vx300s_left/left_finger" kp="400"  user="1"/>
        <position name="left_r_finger_t" ctrllimited="true" ctrlrange="-0.057 -0.01" joint="vx300s_left/right_finger" kp="400"  user="1"/>

        <position name="right_l_finger_t" ctrllimited="true" ctrlrange="0.01 0.057" joint="vx300s_right/left_finger" kp="400"  user="1"/>
        <position name="right_r_finger_t" ctrllimited="true" ctrlrange="-0.057 -0.01" joint="vx300s_right/right_finger" kp="400"  user="1"/>

    </actuator>

    <!-- <keyframe>
        <key qpos="0 -0.96 1.16 0 -0.3 0 0.024 -0.024  0 -0.96 1.16 0 -0.3 0 0.024 -0.024  0.2 0.5 0.05 1 0 0 0"/>
    </keyframe> -->


</mujoco>