<?xml version="1.0" encoding="utf-8"?>
<mujoco>
	<compiler angle="radian" coordinate="local" meshdir="../stls/fetch" texturedir="../textures"></compiler>
	<option timestep="0.002">
		<flag warmstart="enable"></flag>
	</option>

	<include file="shared.xml"></include>
	
	<worldbody>
		<geom name="floor0" pos="0.8 0.25 0" size="0.85 0.7 1" type="plane" condim="3" material="floor_mat"></geom>
		<body name="floor0" pos="0.8 0.25 0">
			<site name="target0" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 0.2" type="ellipsoid"></site>
			<site name="target1" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 0.2" type="ellipsoid"></site>
			<site name="target2" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 0 1 0.2" type="ellipsoid"></site>
			<site name="target3" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 1 0 0.2" type="ellipsoid"></site>
			<site name="target4" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 1 0.2" type="ellipsoid"></site>
			<site name="final_arm_target" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 0 0 0.5" type="ellipsoid"></site>
		</body>

		<include file="robot.xml"></include>

		<body pos="0.95 0.25 0.25" name="table0">
			<geom size="0.35 0.35 0.25" type="box" mass="2000" material="table_mat"></geom>
		</body>

		<body name="object0" pos="0.025 0.025 0.025">
			<joint name="object0:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object0" material="block_mat" rgba="1 0 0 1" mass="2"></geom>
			<site name="object0" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>
		<body name="object1" pos="0.025 0.025 0.05">
			<joint name="object1:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object1" material="block_mat" rgba="0 1 0 1" mass="2"></geom>
			<site name="object1" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>
		<body name="object2" pos="0.025 0.025 0.075">
			<joint name="object2:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object2" material="block_mat" rgba="0 0 1 1" mass="2"></geom>
			<site name="object2" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>
		<body name="object3" pos="0.025 0.025 0.075">
			<joint name="object3:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object3" material="block_mat" rgba="1 1 0 1" mass="2"></geom>
			<site name="object3" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>
		<body name="object4" pos="0.025 0.025 0.075">
			<joint name="object4:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object4" material="block_mat" rgba="0 1 1 1" mass="2"></geom>
			<site name="object4" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>
		<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"></light>



		<!-- End goal and subgoal Visualizations -->

		<!-- End Goal: Yellow -->
		<body name="end_goal_box1" pos="0 0 0">
			<site type="sphere" size="0.02 0.02 0.02" rgba="1 0 0 1" />
		</body>

		<body name="end_goal_box2" pos="0 0 0">
			<site type="sphere" size="0.02 0.02 0.02" rgba="0 1 0 1" />
		</body>

		<body name="end_goal_box3" pos="0 0 0">
			<site type="sphere" size="0.02 0.02 0.02" rgba="0 0 1 1" />
		</body>


		<!-- Subgoal 1: Purple -->
		<body name="sub_goal_1_box1" pos="0 0 0">
			<site type="capsule" size="0.02 0.02 0.02" rgba="1 0 0 0.75" />
		</body>

		<body name="sub_goal_1_box2" pos="0 0 0">
			<site type="capsule" size="0.02 0.02 0.02" rgba="0 1 0 0.75" />
		</body>

		<body name="sub_goal_1_box3" pos="0 0 0">
			<site type="capsule" size="0.02 0.02 0.02" rgba="0 0 1 0.75" />
		</body>

		<!-- Subgoal 2: Green -->
		<body name="subgoal_2_box1" pos="0 0 0">
			<site type="ellipsoid" size="0.02 0.02 0.02" rgba="1 0 0 0.5" />
		</body>

		<body name="subgoal_2_box2" pos="0 0 0">
			<site type="ellipsoid" size="0.02 0.02 0.02" rgba="0 1 0 0.5" />
		</body>

		<body name="subgoal_2_box3" pos="0 0 0">
			<site type="ellipsoid" size="0.02 0.02 0.02" rgba="0 0 1 0.5" />
		</body>

		<!-- Subgoal 3: Red -->
		<body name="subgoal_3_box1" pos="0 0 0">
			<site type="box" size="0.02 0.02 0.02" rgba="1 1 0 0.5" />
		</body>

		<body name="subgoal_3_box2" pos="0 0 0">
			<site type="box" size="0.02 0.02 0.02" rgba="1 1 0 0.5" />
		</body>

		<body name="subgoal_3_box3" pos="0 0 0">
			<site type="box" size="0.02 0.02 0.02" rgba="1 1 0 0.5" />
		</body>


		<body name="gripper_pos_goal" pos="0 0 0">
			<site type="box" size="0.02 0.02 0.02" rgba="0 0 0 0.5" />
		</body>


	</worldbody>
	<actuator>
		<position ctrllimited="true" ctrlrange="0 0.2" joint="robot0:l_gripper_finger_joint" kp="30000" name="robot0:l_gripper_finger_joint" user="1"></position>
		<position ctrllimited="true" ctrlrange="0 0.2" joint="robot0:r_gripper_finger_joint" kp="30000" name="robot0:r_gripper_finger_joint" user="1"></position>
	</actuator>
</mujoco>
