<mujoco>
    <include file="shared.xml" />

    <contact>
        <exclude body1="gripper_r_finger_r" body2="gripper_r_finger_l"/>
        <exclude body1="gripper_l_finger_r" body2="gripper_l_finger_l"/>
    </contact>

    <equality>
        <weld active="false" body1="yumi_mocap_l" body2="gripper_l_center" solimp="0.9 0.95 0.001" solref="0.01 1"/>
        <weld active="false" body1="yumi_mocap_r" body2="gripper_r_center" solimp="0.9 0.95 0.001" solref="0.01 1"/>
    </equality>

    <worldbody>

        <body name="floor" pos="0 0 0">
			<site name="target_l" pos="0 0.1 0.5" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
			<site name="target_r" pos="0 -0.1 0.5" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
		</body>

		<body pos="0 0 -0.01" name="table">
            <geom rgba="1 1 1 1" name="table" type="box" mass="2000" size="0.4 0.5 0.01"/>
            <placeholder name="rotating_platform"/>
            <placeholder name="button"/>
            <placeholder name="object_box"/>
        </body>

        <body mocap="true" name="yumi_mocap_l" pos="0 0 0">
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7" size="0.005 0.005 0.005" 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"/>
        </body>

        <body mocap="true" name="yumi_mocap_r" pos="0 0 0">
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7" size="0.005 0.005 0.005" 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"/>
        </body>

        <body name="yumi_base_link" pos="-0.4 0 0">
            <inertial pos="0 0 0" mass="70" diaginertia="1.2869 1.2236 0.9868"/>
            <body name="yumi_body" pos="0 0 0">
                <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="15" diaginertia="0.6 0.5 0.3" />
                <geom type="mesh" rgba="0.8 0.8 0.8 1" mesh="coarse_body"/>
                <include file="left_arm.xml" />
                <include file="right_arm.xml" />
            </body>
        </body>

        <placeholder name="object"/>

        <light directional="true" ambient="0.2 0.2 0.2" diffuse="0.8 0.8 0.8" specular="0.3 0.3 0.3" castshadow="false" pos="0 0 4" dir="0 0 -1" name="light0"/>
    </worldbody>

    <actuator>
        <include file="left_actuators.xml"/>
        <include file="right_actuators.xml"/>
    </actuator>
</mujoco>
