<?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.001">
        <flag warmstart="enable"></flag>
    </option>
    <include file="shared.xml"></include>
    <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.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>


        <body name="box_hole" pos="1.605 0.25 0.68">
            <joint name="box_hole:joint" type="free" limited="false"></joint>
            <geom name="box_left" pos="0 0 0" size="0.065 0.002 0.04" type= "box" rgba="0.8 0 0 1" mass ="1" condim="4" />
            <geom name ="box_right" pos="0 0.149 0" size="0.065 0.002 0.04" type="box" rgba="0.8 0 0 1" mass ="2" condim="4" />
            <geom name="box_forward" pos="0.064 0.074 0" size="0.002 0.075 0.04" type="box" rgba="0.8 0 0 1" mass ="2" condim="4" />
            <geom name="box_backward" pos="-0.064 0.074 0" size="0.002 0.075 0.04" type="box" rgba="0.8 0 0 1" mass ="2" condim="4" />
            <geom name="box_bottom" pos="-0 0.074 -0.038" size="0.065 0.075 0.002" type="box" rgba="0.8 0 0 1" mass ="2" condim="4"/>
            <site name="target0" pos="0 0.075 -0.035" size="0.0475 0.001" rgba="0 0 0 0" type="cylinder"></site>
        </body>


        <body name="object0" pos="1.4 0.25 0.75">
            <joint name="object0:joint" type="free" limited="false"></joint>
            <geom name="object0_target" type="cylinder" pos="0 0 -0.05" size="0.03 0.035" rgba="0.6 0.8 0.5 1" mass ="0.1" condim="3"  />
            <site name="object0" pos="0 0 -0.05" size="0.03 0.035" rgba="0 0 0 0" type="cylinder"></site>
            <body name="B0" pos="0 0 0" euler="0 0 0 ">
                <joint name="B0:joint" type="slide" limited="true" axis="0 0 1" damping="0.05" range="0.0001 0.0001001" solimpfriction="0.98 0.98 0.95" frictionloss="1"></joint>
                <geom type="capsule" size="0.002 0.03" rgba="0 0 0 1" mass="0.001" condim="4"/>
                <body name="B1" pos="0 0 0.04" euler="0 3.14 0 ">
                    <joint name="B1:joint1" type="hinge" axis="1 0 0" range="-0.1 0.1"  frictionloss="1"></joint>
                    <joint name="B1:joint2" type="hinge" axis="0 1 0" range="-0.1 0.1"  frictionloss="1"></joint>
                    <joint name="B1:joint3" type="hinge" axis="0 0 1" range="-0.1 0.1"  frictionloss="1"></joint>
                    <geom type="capsule" size="0.002 0.004" rgba="1 0 0 0" mass="0.001" condim="4"/>
                </body>
            </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>
        <weld body1="right_hand" body2="B1" solimp="0.99 0.99 0.99" solref="0.02 1"></weld>
        <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>
