<!-- ======================================================
	This file is part of MuJoCo.     
	Copyright 2009-2016 Roboti LLC.
	
	Model 		:: arm_claw
		
	Mujoco		:: Advanced physics simulation engine
		Source		: www.roboti.us
		Version		: 1.31
		Released 	: 23Apr16
		
	Author		:: Vikash Kumar
		Contacts 	: kumar@roboti.us
		Last edits 	: 30Apr16, 30Nov'15, 5Nov'15
====================================================== -->
<mujoco model="arm_claw (v1.31)">
	<compiler 	inertiafromgeom="true"
				angle="radian"/>
			
	<size 	nuser_jnt="1"
			nuser_geom="1"/>

	<option	timestep="0.005"
			gravity="0 0 -3"
			viscosity="0"
			iterations="20"/>
	
	<asset>
		<material name="MatGnd" specular=".3" shininess=".3" reflectance="0.3" rgba=".5 .55 .5 1"/>
	</asset>
	
	<default>
		<joint limited="true" damping="1" stiffness="0" armature=".1" user="0"/>
		<geom solref="0.008 1" solimp="0.95 0.95 0.01" margin="0.001" user="0"/>
		<motor ctrllimited="true" ctrlrange="-1 1"/>
		<position ctrllimited="false"/>
		
		<default class='0'>
			<geom friction="1 0.005 0.001" rgba=".75 .6 .5 1" contype="1" conaffinity="0"/>
		</default>

		<default class='1'>
			<geom friction="1 0.005 0.001" rgba=".7 .6 .55 1" contype="1" conaffinity="0"/>
		</default>
		
		<default class='arm'>
			<joint damping="5"/>
		</default>
	</default>

	<worldbody>
		<light directional="true" cutoff="40" exponent="10" diffuse=".4 .4 .4" specular=".7 .7 .7" pos="0 0 1.5" dir="0 0 -1"/>
		<geom name="tableTop" material="MatGnd" type="plane" pos="0 0 0" size="0.4 0.6 0.5" rgba=".6 .6 .5 1" contype="1" conaffinity="1"/>
	                                                                
		<!--  ================= ARM ================= /-->
		<body name="board1" pos="-0.7 0 0.5" quat="0 0 0 0">
			
			<geom type="box" rgba="1 1 1 0.15" size="0.035 0.6 0.15" class="0"/>
			<body name="upperarm" pos="0 -0.4 0">
				<geom class="0" type="capsule" fromto="0 0 0 0 0 -0.345" size="0.05"/>
				<joint name='slidex1' type='slide' axis='0 1 0' range='-0.1 1'/>
				<joint name='slidey1' type='slide' axis='0 0 1' range='-0.1 0.2'/>
				<joint name='slidez1' type='slide' axis='1 0 0' range='0 0.25'/>
				
				<body name="forearm" pos="0 0 -0.36">
					<geom class="0" type="capsule" fromto="0 0 0 0.225 0 0" size="0.035"/>
					
		<!--  ================= Palm ================= /-->

					<body name="claw" pos="0.27 0 0" quat="0 1 0 0">
						<geom class="1" name="Geomclaw" type="capsule" fromto="0 .02 0 0 -.02 0" size="0.01" />
						<joint name="w_abduction" type="hinge" pos="0 0 0" axis="0 0 1" range="-.5 .5" user="006"/>
						<joint name="w_flexion" type="hinge" pos="0 0 0" axis="0 -1 0" range="-1.05 1.05" user="007"/>

						<body name="rightclaw" pos=".01 -.02 0" >
							<geom name="rightclaw_it" class="1" type="capsule" fromto="0 0 0.015 .03 -.02 0.015" 	size="0.005" />
							<geom name="rightclaw_ot" class="1" type="capsule" fromto=".03 -.02 0.015 .07 0 0.015" size="0.005" />
							<geom name="rightclaw_ib" class="1" type="capsule" fromto="0 0 -0.015 .03 -.02 -0.015" 	size="0.005" />
							<geom name="rightclaw_ob" class="1" type="capsule" fromto=".03 -.02 -0.015 .07 0 -0.015" size="0.005" />
							
						</body>

						<body name="leftclaw" pos=".01 .02 0">
							<geom name="leftclaw_it" class="1" type="capsule" fromto="0 0 0.015 .03 .02 0.015" 	size="0.005" />
							<geom name="leftclaw_ot" class="1" type="capsule" fromto=".03 .02 0.015 .07 0 0.015" size="0.005" />
							<geom name="leftclaw_ib" class="1" type="capsule" fromto="0 0 -0.015 .03 .02 -0.015" 	size="0.005" />
							<geom name="leftclaw_ob" class="1" type="capsule" fromto=".03 .02 -0.015 .07 0 -0.015" size="0.005" />
							<joint name="lc_close" type="hinge" pos="0 0 0" axis="0 0 1" range="-1.05 1.05" user="009"/>
						</body>
					
					</body>
				
				</body>
			
			</body>
		</body>
                                                           
		<!--  ================= ARM 2 ================= /-->

		<body name="board2" pos="0.7 0 0.5" quat="0 0 0 0">
			<geom type="box" rgba="1 1 1 0.15" size="0.035 0.6 0.15" class="0"/>
			<joint class='arm' name="s_abduction" type="hinge" pos="0 0 0" axis="1 0 0" range="-1.57 .7" user="011"/>
			<body name="upperarm2" pos="0 -0.4 0" quat="0 0 0 -2">
				<geom class="0" type="capsule" fromto="0 0 0 0 0 -0.345" size="0.05"/>
				
			
				<joint name='slidex2' type='slide' axis='0 1 0' range='-1 0.1'/>
				<joint name='slidey2' type='slide' axis='0 0 1' range='-0.1 0.2'/>
				<joint name='slidez2' type='slide' axis='1 0 0' range='0 0.25'/>
				<body name="forearm2" pos="0 0 -0.36">
					<geom class="0" type="capsule" fromto="0 0 0 0.225 0 0" size="0.035"/>
				
					
		<!--  ================= Palm 2 ================= /-->

					<body name="claw2" pos="0.27 0 0" quat="0 1 0 0">
						<geom class="1" name="Geomclaw2" type="capsule" fromto="0 .02 0 0 -.02 0" size="0.01" />
						<joint name="w_abduction2" type="hinge" pos="0 0 0" axis="0 0 1" range="-.5 .5" user="016"/>
						<joint name="w_flexion2" type="hinge" pos="0 0 0" axis="0 -1 0" range="-1.05 1.05" user="017"/>

						<body name="rightclaw2" pos=".01 -.02 0" >
							<geom name="rightclaw_it2" class="1" type="capsule" fromto="0 0 0.015 .03 -.02 0.015" 	size="0.005" />
							<geom name="rightclaw_ot2" class="1" type="capsule" fromto=".03 -.02 0.015 .07 0 0.015" size="0.005" />
							<geom name="rightclaw_ib2" class="1" type="capsule" fromto="0 0 -0.015 .03 -.02 -0.015" 	size="0.005" />
							<geom name="rightclaw_ob2" class="1" type="capsule" fromto=".03 -.02 -0.015 .07 0 -0.015" size="0.005" />
							<joint name="rc_close2" type="hinge" pos="0 0 0" axis="0 0 1" range="-1.05 1.05" user="018"/>
						</body>

						<body name="leftclaw2" pos=".01 .02 0">
							<geom name="leftclaw_it2" class="1" type="capsule" fromto="0 0 0.015 .03 .02 0.015" 	size="0.005" />
							<geom name="leftclaw_ot2" class="1" type="capsule" fromto=".03 .02 0.015 .07 0 0.015" size="0.005" />
							<geom name="leftclaw_ib2" class="1" type="capsule" fromto="0 0 -0.015 .03 .02 -0.015" 	size="0.005" />
							<geom name="leftclaw_ob2" class="1" type="capsule" fromto=".03 .02 -0.015 .07 0 -0.015" size="0.005" />
							<joint name="lc_close2" type="hinge" pos="0 0 0" axis="0 0 1" range="-1.05 1.05" user="019"/>
						</body>
					
					</body>
				
				</body>
			
			</body>
		</body>
		<!--  ================= OBJECT ================= pos="0 -0.4 0.06" quat="0 0 0 0" /-->
		 
		<!-- LARGE ELLIPSOID
		<body name="object" pos="0 -0.4 0.035" quat="0 0 0 0" euler="0 0 0">
			<geom  type="ellipsoid" rgba="1 0 0.5 1" size="0.25 0.045 0.045" friction="5" user="1"/>
			<joint name="fr" type="free" limited='false' damping="0" armature="0" />
		</body>
		-->
		
		<!-- LONG CAPSULE -->
		<body name="object" pos="0 -0.41 0.055" quat="0 0 0 0" euler="1.5 1.6 0">
			<geom  type="capsule" rgba="1 0 0.5 1" size="0.030 0.25 0.025" friction="0" user="1"/>
			<joint name="fr" type="free" limited='false' damping="0.2" armature="0" />
		</body>
		
		
		<!-- SMALL CAPSULE 
		<body name="object" pos="0 -0.41 0.055" quat="0 0 0 0" euler="1.5 1.6 0">
			<geom  type="capsule" rgba="1 0 0.5 1" size="0.030 0.13 0.025" friction="0" user="1"/>

			<joint name="fr" type="free" limited='false' damping="0.2" armature="0" />

		</body>
		-->

		<!-- LARGE CUBOID WITH HEIGHT
		<body name="object" pos="0 -0.4 0.045" quat="0 0 0 0" euler="0 0 0">
			<geom  type="box" rgba="1 0.7 0 1" size="0.25 0.025 0.045" user="1"/>
			<joint name="fr" type="free" limited='false' damping="0" armature="0" />
		</body>
		-->
		
		<!-- SMALL CUBOID WITH HEIGHT 
		<body name="object" pos="0 -0.4 0.045" quat="0 0 0 0" euler="0 0 0">

			<geom  type="box" rgba=" 0 0 1 1" size="0.15 0.025 0.045" user="1"/>

			<joint name="fr" type="free" limited='false' damping="0" armature="0" />
		</body>
		-->
		
		<!-- LONG CUBOID 
		<body name="object" pos="0 -0.4 0.026" quat="0 0 0 0" euler="0 0 0">
			<geom  type="box" rgba=" 1 0.2 0 1" size="0.25 0.025 0.025" user="1"/>
			<joint name="fr" type="free" limited='false' damping="0" armature="0" />
		</body>
		-->
		
		<!--SMALL CUBOID WITHOUT HEIGHT -->
		<body name="object" pos="0 -0.4 0.045" quat="0 0 0 0" euler="0 0 0">
			<geom  type="box" rgba=" 0 1 1 1" size="0.15 0.025 0.025" user="1"/>
			<joint name="fr" type="free" limited='false' damping="0" armature="0" />
		</body>

		
		<body name="goal" pos="0 0.4 0.01" quat="0 0 0 0" >
			<geom  type="box" rgba="0 1 0 0.1" size="0.05 0.05 0.000001" user="1" contype="0" conaffinity="1"/>
			<joint name="fr2" type="free" limited='false' damping="2" armature="0"/>
		</body>
	
	</worldbody>
	
	<actuator>


		<position joint="slidex1" name="sliderx1" kp="10" ctrlrange="-1 1"/>
		<position joint="slidey1" name="slidery1" kp="10" ctrlrange="-1 1"/>
		<position joint="slidez1" name="sliderz1" kp="10" ctrlrange="-1 1"/>
		
		<position joint='lc_close' 		name='Alc_close' 	kp="10"  ctrlrange='-1.05 1.05'/>

		<position joint="slidex2" name="sliderx2" kp="10" ctrlrange="-1 1"/>
		<position joint="slidey2" name="slidery2" kp="10" ctrlrange="-1 1"/>
		<position joint="slidez2" name="sliderz2" kp="10" ctrlrange="-1 1"/>
		
	
		<position joint='lc_close2' 		name='Alc_close2' 	kp="10"  ctrlrange='-1.05 1.05'/>
	</actuator>

</mujoco>
