<?xml version="1.0" encoding="utf-8"?>
<mujoco> 
	<compiler angle="radian" coordinate="local" meshdir="fetch" texturedir="texture"></compiler>
	<size nconmax="2000" njmax="500"/>
	<option timestep="0.002">
		<flag warmstart="enable"></flag>
	</option>
	<include file="shared.xml"></include>
	<default>
        <default class="board">
            <geom type="box" material="puck_mat" />
        </default>
    </default>

    <contact>
        <exclude body1='nail_board' body2='nail1'/>
        <exclude body1='nail_board' body2='nail2'/>
        <exclude body1='nail_board' body2='nail3'/>
        <exclude body1='nail_board' body2='nail4'/>
    </contact>

	<worldbody>
		<body name="floor0" pos="0 0 0">
            <geom name="floorgeom0" pos="1.2 -2.0 0" size="1.0 10.0 1" type="plane" condim="3" material="floor_mat"></geom>
        </body>

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

		<body pos="0.75 0 0.71" name="pedestal0">
            <geom name="pedestalgeom0" size="0.4 0.4 0.01" type="box" mass="2000" material="pedestal_mat"></geom>
            <site pos="0.30 0.30 0" size="0.075 0.075 0.002" type="box" name="robotmountsite0" rgba="0.55 0.54 0.53 1" />
        </body>
        
        <body pos="1.555 0.3 0.53625" name="table0">
            <geom name="tablegeom0" size="0.3 0.6 0.04375" type="box" material="table_mat_white" density="2000" friction="1 0.005 0.0001"></geom>
            <!-- <site pos="-0.1 0 0.19" size="0.3 0.3 0.002" type="box" name="vis" rgba="0 0 0 0.2" /> -->
        </body>


        <!-- ======= Nail ======= -->
        <body name="nail_board" pos="1.555 0.3 0.73">
            <joint name="nail_board:joint" type="free" limited="false"></joint>
            <geom name="board" class="board" size="0.045 0.1 0.045" rgba="0.33 0.77 0.78 1" mass="500" condim="4" material="puck_mat"/>

            <site name="nail_goal1" pos="0.025 0.077 0.05" size="0.023 0.005" type="cylinder"  rgba="1 0.8 0.8 0" />
            <body name="nail1" pos="0.025 0.077 0.005">
                <joint name="nail_dir1" pos="0 0 0" axis="0 0 -1" limited="true" type="slide" range="-0.046 -0.005" frictionloss="2000.5" />
                <geom size="0.023 0.005 " pos="0 0 0.04" type="cylinder" rgba="0.6 0.79 0.16 1" condim="4"/>
                <geom size="0.012 0.023 " pos="0 0 0.015" type="cylinder" rgba="0.6 0.79 0.16 1" condim="4"/>
                <site name="nail_target1" pos="0 0 0.04" size="0.02 0.005" type="cylinder" rgba="0.6 0.79 0.16 1" />
            </body>

            <site name="nail_goal2" pos="-0.025 0.033 0.05" size="0.023 0.005" type="cylinder"  rgba="1 0.8 0.8 0" />
            <body name="nail2" pos="-0.025 0.033 0.005">
                <joint name="nail_dir2" pos="0 0 0" axis="0 0 -1" limited="true" type="slide" range="-0.046 -0.005" frictionloss="2000.5" />
                <geom size="0.023 0.005 " pos="0 0 0.04" type="cylinder" rgba="0.87 0.1 0 1" condim="4"/>
                <geom size="0.012 0.023 " pos="0 0 0.015" type="cylinder" rgba="0.87 0.1 0 1" condim="4"/>
                <site name="nail_target2" pos="0 0 0.04" size="0.02 0.005" type="cylinder" rgba="0.87 0.1 0 1" />
            </body>

            <site name="nail_goal3" pos="-0.025 -0.033 0.05" size="0.023 0.005" type="cylinder"  rgba="1 0.8 0.8 0" />
            <body name="nail3" pos="-0.025 -0.033 0.005">
                <joint name="nail_dir3" pos="0 0 0" axis="0 0 -1" limited="true" type="slide" range="-0.046 -0.005" frictionloss="2000.5" />
                <geom size="0.023 0.005 " pos="0 0 0.04" type="cylinder" rgba="1 0.6 0 1" condim="4"/>
                <geom size="0.012 0.023 " pos="0 0 0.015" type="cylinder" rgba="1 0.6 0 1" condim="4"/>
                <site name="nail_target3" pos="0 0 0.04" size="0.02 0.005" type="cylinder" rgba="1 0.6 0 1" />
            </body>

            <site name="nail_goal4" pos="0.025 -0.077 0.05" size="0.023 0.005" type="cylinder"  rgba="1 0.8 0.8 0" />
            <body name="nail4" pos="0.025 -0.077 0.005">
                <joint name="nail_dir4" pos="0 0 0" axis="0 0 -1" limited="true" type="slide" range="-0.046 -0.005" frictionloss="2000.5" />
                <geom size="0.023 0.005 " pos="0 0 0.04" type="cylinder" rgba="1 0.9 0 1" condim="4"/>
                <geom size="0.012 0.023 " pos="0 0 0.015" type="cylinder" rgba="1 0.9 0 1" condim="4"/>
                <site name="nail_target4" pos="0 0 0.04" size="0.02 0.005" type="cylinder" rgba="1 0.9 0 1" />
            </body>
            
        </body>

		<light directional="true" ambient="0.1 0.1 0.1" diffuse="0 0 0" specular="0 0 0" castshadow="false" pos="1.65 0 10" dir="-0.57 -0.57 -0.57" name="light0"></light>
        <light directional="true" ambient="0.1 0.1 0.1" diffuse="0 0 0" specular="0 0 0" castshadow="false" pos="0 -4 4" dir="0 1 -0.1" name="light1"></light>
        <light directional="true" ambient="0.05 0.05 0.05" diffuse="0 0 0" specular="0 0 0" castshadow="false" pos="2.13 1.6 2.5" name="light2"></light>
        <light pos="0 0 2" dir="0.2 0.2 -0.8" directional="true"  diffuse="0.3 0.3 0.3"  castshadow="false" name="light3"></light>
        
        <camera fovy="50" name="third_person" pos="2.10 0.3 1.4" euler="0 0.83 1.57"/>
    </worldbody>
    
    <equality>
        <connect body2="left_finger" body1="left_inner_knuckle" anchor="0.0 0.035 0.042" solimp="0.9 0.95 0.001 0.5 2" solref="0.0002 1.0" ></connect>
        <connect body2="right_finger" body1="right_inner_knuckle" anchor="0.0 -0.035 0.042" solimp="0.9 0.95 0.001 0.5 2" solref="0.0002 1.0" ></connect>
        <joint joint1="left_inner_knuckle_joint" joint2="right_inner_knuckle_joint"></joint>
    </equality>
    <actuator>
        <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="left_inner_knuckle_joint" gear="200.0"/>
        <motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="right_inner_knuckle_joint" gear="200.0"/>
    </actuator>
</mujoco>
