<?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.75 0" size="0.85 0.70 1" type="plane" condim="3" material="floor_mat"></geom>
		<body name="floor0" pos="0.8 0.75 0">
			<site name="target0" pos="0 -0.2 0.5" size="0.02 0.02 0.02" rgba="0.0 0.5 0.0 1" type="sphere"></site>
			<site name="target1" pos="0 -0.1 0.5" size="0.02 0.02 0.02" rgba="0.5 0.0 0.0 1" type="sphere"></site>
			<site name="target2" pos="0 0.0 0.5" size="0.02 0.02 0.02" rgba="0.0 0.5 0.5 1" type="sphere"></site>
			<site name="target3" pos="0 0.1 0.5" size="0.02 0.02 0.02" rgba="0.5 0.0 0.5 1" type="sphere"></site>
			<site name="target4" pos="0 0.2 0.5" size="0.02 0.02 0.02" rgba="0.5 0.5 0.0 1" type="sphere"></site>
		</body>

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

		<body pos="1.3 0.75 0.2" name="table0">
		<geom pos="0 0 0" size="0.25 0.35 0.2" type="box" mass="2000" material="table_mat" group="1"></geom>
			<geom pos="0 0.35 0.2" size="0.26 0.01 0.05" type="box" group="1" material="table_mat"/>
			<geom pos="0 -0.35 0.2" size="0.26 0.01 0.05" type="box" group="1" material="table_mat"/>
			<geom pos="0.25 0 0.2" size="0.01 0.35 0.05" type="box" group="1" material="table_mat"/>
			<geom pos="-0.25 0 0.2" size="0.01 0.35 0.05" type="box" group="1" material="table_mat"/>
		</body>

		<!-- <body pos="1.6 0.75 0.2" name="table0">
			<geom size="0.55 1.20 0.2" 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_0_mat" 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.100 0.025 0.025">
			<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_1_mat" 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.200 0.025 0.025">
			<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_2_mat" 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.300 0.025 0.025">
			<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_3_mat" 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.400 0.025 0.025">
			<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_4_mat" 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>

		<body name="object5" pos="0.500 0.025 0.025">
			<joint name="object5:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object5" material="block_5_mat" mass="2"></geom>
			<site name="object5" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>

		<body name="object6" pos="0.600 0.025 0.025">
			<joint name="object6:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object6" material="block_6_mat" mass="2"></geom>
			<site name="object6" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>

		<body name="object7" pos="0.700 0.025 0.025">
			<joint name="object7:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object7" material="block_7_mat" mass="2"></geom>
			<site name="object7" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>

		<body name="object8" pos="0.800 0.025 0.025">
			<joint name="object8:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object8" material="block_8_mat" mass="2"></geom>
			<site name="object8" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>

		<body name="object9" pos="0.900 0.025 0.025">
			<joint name="object9:joint" type="free" damping="0.01"></joint>
			<geom size="0.025 0.025 0.025" type="box" condim="3" name="object9" material="block_9_mat" mass="2"></geom>
			<site name="object9" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"></site>
		</body>

		<!-- Adding mocap for the ai objects -->

		<body mocap="true" name="object5:mocap" pos="0.500 0.025 0.025">
			<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>
			<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>
			<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>
			<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>
		</body>

		<body mocap="true" name="object6:mocap" pos="0.600 0.025 0.025">
			<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>
			<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>
			<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>
			<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>
		</body>

		<body mocap="true" name="object7:mocap" pos="0.700 0.025 0.025">
			<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>
			<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>
			<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>
			<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>
		</body>

		<body mocap="true" name="object8:mocap" pos="0.800 0.025 0.025">
			<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>
			<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>
			<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>
			<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>
		</body>

		<body mocap="true" name="object9:mocap" pos="0.900 0.025 0.025">
			<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>
			<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>
			<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>
			<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>
		</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>
	</worldbody>

	<!-- Adding mocap for the object -->	
	<equality>
		<weld body1="object5:mocap" body2="object5" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
		<weld body1="object6:mocap" body2="object6" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
		<weld body1="object7:mocap" body2="object7" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
		<weld body1="object8:mocap" body2="object8" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
		<weld body1="object9:mocap" body2="object9" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
	</equality>
	
	<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>