<?xml version="1.0" encoding="utf-8"?>
<mujoco>
	<compiler angle="radian" coordinate="local" meshdir="../stls/fetch" texturedir="../textures"></compiler>
	<option timestep="0.002" iterations="200"><!--todo check if this is correct(used for moving obstacle)-->
		<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" rgba="1 1 1 1"></geom>
		<body name="floor0" pos="0.8 0.75 0">
			<site name="target0" pos="0 0 0.5" size="0.025 0.025" rgba="1 0 0 1" type="cylinder"></site>
		</body>

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

		<body pos="1.3 0.75 0.2" name="table0">
			<geom size="0.25 0.25 0.2" type="box" mass="2000" material="table_mat" solref="0.01 1" rgba="1 1 1 1"></geom>
			<site name="target_center" pos="-0.07 -0.2 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
			<site name="init_center" pos="-0.07 0.2 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
			<site name="init_1" pos="-1.875 0.45 0.2" size="0.02 0.02 0.02" rgba="0 1 0 0" type="sphere"></site>
			<site name="init_2" pos="-1.25 0.45 0.2" size="0.02 0.02 0.02" rgba="0 1 0 0" type="sphere"></site>
			<site name="init_3" pos="-0.625 0.45 0.2" size="0.02 0.02 0.02" rgba="0 1 0 0" type="sphere"></site>
			<site name="init_4" pos="0 0.45 0.2" size="0.02 0.02 0.02" rgba="0 1 0 0" type="sphere"></site>
			<site name="mark1" pos="0.625 0.45 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
			<site name="mark2" pos="1.25 0.45 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
			<site name="mark3" pos="1.875 0.45 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
			<site name="mark4" pos="-1.875 -0.45 0.2" size="0.02 0.02 0.02" rgba="0 0 1 0" type="sphere"></site>
		</body>

		<body pos="1.3 0.75 0.6" name="space0">
			<site name="spacesite" size="0.25 0.25 0.2" rgba="0 0 1 0" type="box"></site>
		</body>
		<body pos="1.395 0.872 0.425" name="obstacle">
			<!--limited="true" range="-.05 .05"  damping='0.01' armature='0' frictionloss='0.1'-->
			<geom name="o" pos="0 0 0" size="0.09 0.03 0.025" type="box" solref="0.01 1"
				  rgba="0 0 1 1" mass="2000" material="table_mat"></geom>
			<joint name="obstacle:joint" type="slide" axis="1 0 0" limited="true" damping="0.01" range="-0.065 0.065" margin="0.03"></joint>

		</body>
		<!--body pos="1.395 0.872 0.425" name="spaceObstacle">
			<site name="siteObstacle" size="0.155 0.03 0.025" rgba="0 0 1 0.2" type="box"></site>
			<site name="smark1" pos="-0.155 -0.03 -0.025" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="smark2" pos="-0.155 0.03 -0.025" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="smark3" pos="0.155 -0.03 -0.025" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="smark4" pos="0.155 0.03 -0.025" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
		</body>

		<body pos="1.4 0.7 0.41" name="spaceGoal">
			<site name="siteGoalp" size="0.03 0.03 0.01" rgba="0 1 0 0.4" type="box"></site>
			<site name="emark1" pos="-0.03 -0.03 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="emark2" pos="-0.03 0.03 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="emark3" pos="0.03 -0.03 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="emark4" pos="0.03 0.03 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
		</body>

		<body pos="1.1 0.65 0.41" name="spacePuck">
			<site name="sitePuck" size="0.04 0.04 0.01" rgba="1 0 0 0.2" type="box"></site>
			<site name="pmark1" pos="-0.04 -0.04 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="pmark2" pos="-0.04 0.04 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="pmark3" pos="0.04 -0.04 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
			<site name="pmark4" pos="0.04 0.04 -0.01" size="0.01 0.01 0.01" rgba="0 0 0 1" type="sphere"></site>
		</body-->




		<!--Setting contype and coaffinity to avoid collision between obstacle2 and obstacle 3-->
		<body pos="1.3 0.51 0.44" name="obstacle2">
			<geom name="o2" pos="0 0 0" size="0.3 0.02 0.04"  type="box" solref="0.01 1"
				  rgba="0.5 0. 0.5 1" mass="2000" material="table_mat" contype="1" conaffinity="0"></geom>
		</body>

		<body pos="1.185 0.68 0.44" name="obstacle3">
			<geom name="o3" pos="0 0 0" size="0.02 0.16 0.04" type="box" solref="0.01 1"
				  rgba="0.8 0.8 0. 1" mass="2000" material="table_mat" contype="1" conaffinity="0"></geom>
		</body>


		<body name="object0" pos="1.095 0.68 0.425">
			<joint name="object0:joint" type="free" damping="0.001"></joint>
			<geom name="object0" size="0.04 0.02" rgba="1 0 0 1" type="cylinder" condim="3" mass="50"
				  solref="0.01 1" material="block_mat"></geom>

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



		<!--body name="lookat" pos="1.3 .75 0.2">
			<geom size="0.005 0.005 0.005" type="sphere" rgba="1 0 1 0"></geom>
		</body>

		<body name="camera rig" pos="1.3 .75 2.5">
			<camera name="cam_top" mode="targetbody" target="lookat" pos="0 0 0" fovy="15"></camera>
		</body-->

		<body name="lookat" pos="1.25 .65 0.8">
			<geom size="0.005 0.005 0.005" type="sphere" rgba="1 0 1 0"></geom>
		</body>

		<body name="camera rig" pos="1.6 .85 1.55">
			<camera name="cam_top" mode="targetbody" target="lookat" pos="0 0 0" fovy="50"></camera>
		</body>

		<!--body name="lookat" pos="1.22 0.85 0.425">
			<geom size="0.005 0.005 0.005" type="sphere" rgba="1 0 1 0"></geom>
		</body>

		<body name="camera rig" pos="1.22 1.55 1.2">
			<camera name="cam_top" mode="targetbody" target="lookat" pos="0 0 0" fovy="40"></camera>
		</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>
		<light directional="true" ambient="0.2 0.2 0.2" diffuse="0.2 0.2 0.2" specular="0.3 0.3 0.3" castshadow="false" pos="0 0 -300" dir="0 0 1" name="light1"></light>




	</worldbody>
</mujoco>
