<?xml version="1.0" encoding="utf-8"?>
<mujoco>
	<!-- <compiler angle="radian" coordinate="local" meshdir="../stls/fetch" texturedir="../textures"></compiler> -->
    <compiler angle="radian" coordinate="local" meshdir="fetch" texturedir="../textures"></compiler>
	<option timestep="0.002">
		<flag warmstart="enable"></flag>
	</option>

    <asset>
        <texture type="skybox" builtin="gradient" rgb1="0.44 0.85 0.56" rgb2="0.46 0.87 0.58" width="32" height="32"></texture>
        <texture name="texture_block" file="block.png" gridsize="3 4" gridlayout=".U..LFRB.D.."></texture>

        <texture builtin="flat" name="texgeom" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
        <texture builtin="flat" name="texgeom_small" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="gripper_finger_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="gripper_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
        <texture builtin="flat" name="background_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="arm_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="head_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="torso_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>
		<texture builtin="flat" name="base_tex" height="32" width="32" rgb1="1 1 1" type="cube"></texture>


        <material name="floor_mat" specular="0" shininess="0.5" reflectance="0" rgba="0.2 0.2 0.2 1" texture="texgeom"></material>
        <material name="table_mat" specular="0" shininess="0.5" reflectance="0" rgba="0.93 0.93 0.93 1" texture="texgeom"></material>
        <material name="smalltable_mat" specular="0" shininess="0.5" reflectance="0" rgba="0.93 0.93 0.93 1" texture="texgeom_small"></material>
        <material name="block_mat" specular="0" shininess="0.5" reflectance="0" rgba="0.2 0.2 0.2 1" texture="texgeom"></material>
        <material name="puck_mat" specular="0" shininess="0.5" reflectance="0" rgba="0.2 0.2 0.2 1" texture="texgeom"></material>

        <material name="robot0:geomMat" shininess="0.03" specular="0.4" texture="texgeom"></material>
        <material name="robot0:gripper_finger_mat" shininess="0.03" specular="0.4" reflectance="0" texture="gripper_finger_tex"></material>
        <material name="robot0:gripper_mat" shininess="0.03" specular="0.4" reflectance="0" texture="gripper_tex"></material>
        <material name="background:gripper_mat" shininess="0.03" specular="0.4" reflectance="0" texture="background_tex"></material>
        <material name="robot0:arm_mat" shininess="0.03" specular="0.4" reflectance="0"  texture="arm_tex"></material>
        <material name="robot0:head_mat" shininess="0.03" specular="0.4" reflectance="0" texture="head_tex"></material>
        <material name="robot0:torso_mat" shininess="0.03" specular="0.4" reflectance="0" texture="torso_tex"></material>
        <material name="robot0:base_mat" shininess="0.03" specular="0.4" reflectance="0" texture="base_tex"></material>


        
        <mesh file="base_link_collision.stl" name="robot0:base_link"></mesh>
        <mesh file="bellows_link_collision.stl" name="robot0:bellows_link"></mesh>
        <mesh file="elbow_flex_link_collision.stl" name="robot0:elbow_flex_link"></mesh>
        <mesh file="estop_link.stl" name="robot0:estop_link"></mesh>
        <mesh file="forearm_roll_link_collision.stl" name="robot0:forearm_roll_link"></mesh>
        <mesh file="gripper_link.stl" name="robot0:gripper_link"></mesh>
        <mesh file="head_pan_link_collision.stl" name="robot0:head_pan_link"></mesh>
        <mesh file="head_tilt_link_collision.stl" name="robot0:head_tilt_link"></mesh>
        <mesh file="l_wheel_link_collision.stl" name="robot0:l_wheel_link"></mesh>
        <mesh file="laser_link.stl" name="robot0:laser_link"></mesh>
        <mesh file="r_wheel_link_collision.stl" name="robot0:r_wheel_link"></mesh>
        <mesh file="torso_lift_link_collision.stl" name="robot0:torso_lift_link"></mesh>
        <mesh file="shoulder_pan_link_collision.stl" name="robot0:shoulder_pan_link"></mesh>
        <mesh file="shoulder_lift_link_collision.stl" name="robot0:shoulder_lift_link"></mesh>
        <mesh file="upperarm_roll_link_collision.stl" name="robot0:upperarm_roll_link"></mesh>
        <mesh file="wrist_flex_link_collision.stl" name="robot0:wrist_flex_link"></mesh>
        <mesh file="wrist_roll_link_collision.stl" name="robot0:wrist_roll_link"></mesh>
        <mesh file="torso_fixed_link.stl" name="robot0:torso_fixed_link"></mesh>
    </asset>

    <size njmax="500" nconmax="100" />
    <default>
        <joint armature="1" damping="0.1" limited="true"/>
         <default class="robot0:blue">
            <geom rgba="0.086 0.506 0.767 1.0"></geom>
        </default>

        <default class="robot0:grey">
            <geom rgba="0.356 0.361 0.376 1.0"></geom>
        </default>

    </default>
    <asset>
        <mesh name="link_base" file="link_base.STL" />
        <mesh name="link1" file="link1.STL" />
        <mesh name="link2" file="link2.STL" />
        <mesh name="link3" file="link3.STL" />
        <mesh name="link4" file="link4.STL" />
        <mesh name="link5" file="link5.STL" />
        <mesh name="link6" file="link6.STL" />
        <mesh name="link7" file="link7.STL" />
        <mesh name="base_link" file="base_link.STL" />
        <mesh name="left_outer_knuckle" file="left_outer_knuckle.STL" />
        <mesh name="left_finger" file="left_finger.STL" />
        <mesh name="left_inner_knuckle" file="left_inner_knuckle.STL" />
        <mesh name="right_outer_knuckle" file="right_outer_knuckle.STL" />
        <mesh name="right_finger" file="right_finger.STL" />
        <mesh name="right_inner_knuckle" file="right_inner_knuckle.STL" />
    </asset>

	<equality>
        <weld body1="robot0:mocap2" body2="right_hand" solimp="0.9 0.95 0.001" solref="0.02 1"></weld>
    </equality>

	<worldbody>
		<geom name="background" pos="0.8 0.75 0.45" size="0.01 10 10" type="box" condim="3" material="background:gripper_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.02 0.02 0.02" rgba="247 202 0 0" type="sphere"></site>
		</body>

        <body mocap="true" name="robot0:mocap2" pos="0 0 0">
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" 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" size="1 0.005 0.005" type="box"></geom>
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 1 0.001" type="box"></geom>
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0" size="0.005 0.005 1" type="box"></geom>
        </body>
        <!-- <body name="base" pos="0 0 0"> -->

        <body name="base" pos="1 0.75 0.45">
            <geom name="bb" type="mesh" mesh="link_base" material="robot0:base_mat" rgba="1 1 1 1"/>
            <body name="link1" pos="0 0 0.267">
                <inertial pos="-0.0042142 0.02821 -0.0087788" quat="0.917781 -0.277115 0.0606681 0.277858" mass="0.42603" diaginertia="0.00144551 0.00137757 0.000823511" />
                <joint name="joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping="10" frictionloss="1" />
                <geom name="j1" type="mesh" mesh="link1" material="robot0:arm_mat" rgba="1 1 1 1"/>
                <body name="link2" pos="0 0 0" quat="0.707105 -0.707108 0 0">
                    <inertial pos="-3.3178e-05 -0.12849 0.026337" quat="0.447793 0.894132 -0.00224061 0.00218314" mass="0.56095" diaginertia="0.00319151 0.00311598 0.000980804" />
                    <joint name="joint2" pos="0 0 0" axis="0 0 1" limited="true" range="-2.059 2.0944" damping="10" frictionloss="1" />
                    <geom name="j2"  type="mesh"  mesh="link2" material="robot0:head_mat" rgba="1 1 1 1"/>
                    <body name="link3" pos="0 -0.293 0" quat="0.707105 0.707108 0 0">
                        <inertial pos="0.04223 -0.023258 -0.0096674" quat="0.883205 0.339803 0.323238 0.000542237" mass="0.44463" diaginertia="0.00133227 0.00119126 0.000780475" />
                        <joint name="joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping="5" frictionloss="1" />
                        <geom name="j3" type="mesh" mesh="link3" material="robot0:gripper_mat" rgba="1 1 1 1"/>
                        <body name="link4" pos="0.0525 0 0" quat="0.707105 0.707108 0 0">
                            <inertial pos="0.067148 -0.10732 0.024479" quat="0.0654142 0.483317 -0.738663 0.465298" mass="0.52387" diaginertia="0.00288984 0.00282705 0.000894409" />
                            <joint name="joint4" pos="0 0 0" axis="0 0 1" limited="true" range="-0.19198 3.927" damping="5" frictionloss="1" />
                            <geom name="j4" type="mesh" mesh="link4" material="robot0:arm_mat" rgba="1 1 1 1"/>
                            <body name="link5" pos="0.0775 -0.3425 0" quat="0.707105 0.707108 0 0">
                                <inertial pos="-0.00023397 0.036705 -0.080064" quat="0.981064 -0.19003 0.00637998 0.0369004" mass="0.18554" diaginertia="0.00099553 0.000988613 0.000247126" />
                                <joint name="joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping="5" frictionloss="1" />
                                <geom name="j5" type="mesh"  material="robot0:gripper_mat" rgba="1 1 1 1" mesh="link5" />
                                <body name="link6" pos="0 0 0" quat="0.707105 0.707108 0 0">
                                    <inertial pos="0.058911 0.028469 0.0068428" quat="-0.188705 0.793535 0.166088 0.554173" mass="0.31344" diaginertia="0.000827892 0.000768871 0.000386708" />
                                    <joint name="joint6" pos="0 0 0" axis="0 0 1" limited="true" range="-1.69297 3.14159" damping="2" frictionloss="1" />
                                    <geom name="j6" type="mesh" material="robot0:gripper_mat" rgba="1 1 1 1" mesh="link6" />
                                    <body name="link7" pos="0.076 0.097 0" quat="0.707105 -0.707108 0 0">
                                        <inertial pos="-0.000420033 -0.00287433 0.0257078" quat="0.999372 -0.0349129 -0.00605634 0.000551744" mass="0.85624" diaginertia="0.00137671 0.00118744 0.000514968" />
                                        <joint name="joint7" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping="2" frictionloss="1" />
                                        <geom name="j8" material="robot0:gripper_mat" type="mesh" rgba="0.753 0.753 0.753 1" mesh="link7" />
                                        <geom name="j9" material="robot0:gripper_mat" type="mesh" rgba="1 1 1 1" mesh="base_link" />
                                        <body name="left_outer_knuckle" pos="0 0.035 0.059098">
                                            <inertial pos="0 0.021559 0.015181" quat="0.47789 0.87842 0 0" mass="0.033618" diaginertia="1.9111e-05 1.79089e-05 1.90167e-06" />
                                            <joint name="drive_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.85" />
                                            <geom type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="1" contype="0"  mesh="left_outer_knuckle" />
                                            <body name="left_finger" pos="0 0.035465 0.042039">
                                                <inertial pos="0 -0.016413 0.029258" quat="0.697634 0.115353 -0.115353 0.697634" mass="0.048304" diaginertia="1.88037e-05 1.7493e-05 3.56792e-06" />
                                                <joint name="left_finger_joint" pos="0 0 0" axis="-1 0 0" limited="true" range="0 0.85" />
                                                <geom name="j10" material="robot0:gripper_finger_mat" type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="3" contype="2"  mesh="left_finger" />

                                                <body name="right_hand" pos="0 -0.03 0.05" quat="0 0 0 1">
                                                    <!-- This sites were added for visualization. They are all standardized between models-->
                                                    <!-- Position mimics the gripper attachment point (right_hand) -->
                                                    <!--  Y-axis should be perpendicular to grasping motion, and Z-axis should point out of the robot eef -->
                                                    <site name="ee" pos="0 0 0" rgba="0 0 1 1" type="sphere" group="1"/>
                                                    <site name="ee_x" pos="0 0 0" size="0.005 .1"  quat="0.707105 0.707108 0 0 " rgba="1 0 0 0" type="cylinder" group="1"/>
                                                    <site name="ee_z" pos="0 0 0" size="0.005 .1" quat="0.707105 0 0 0.707108" rgba="0 0 1 0" type="cylinder" group="1"/>
                                                    <site name="ee_y" pos="0 0 0" size="0.005 .1" quat="0.707105 0 0.707108 0 " rgba="0 1 0 0" type="cylinder" group="1"/>
                                                    <!-- This camera points out from the eef. -->
                                                    <camera mode="fixed" name="eye_in_hand" pos="-0.05 0 0" quat="0 -0.707108 0.707108 0" fovy="75"/>
                                                    <!-- To add gripper -->
                                                </body>

                                            </body>
                                        </body>
                                        <body name="left_inner_knuckle" pos="0 0.02 0.074098">
                                            <inertial pos="1.86601e-06 0.0220468 0.0261335" quat="0.664139 -0.242732 0.242713 0.664146" mass="0.0230126" diaginertia="8.34216e-06 6.0949e-06 2.75601e-06" />
                                            <joint name="left_inner_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.85" />
                                            <geom type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="1" contype="0"  mesh="left_inner_knuckle" />
                                        </body>
                                        <body name="right_outer_knuckle" pos="0 -0.035 0.059098">
                                            <inertial pos="0 -0.021559 0.015181" quat="0.87842 0.47789 0 0" mass="0.033618" diaginertia="1.9111e-05 1.79089e-05 1.90167e-06" />
                                            <joint name="right_outer_knuckle_joint" pos="0 0 0" axis="-1 0 0" limited="true" range="0 0.85" />
                                            <geom type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="1" contype="0"  mesh="right_outer_knuckle" />
                                            <body name="right_finger" pos="0 -0.035465 0.042039">
                                                <inertial pos="0 0.016413 0.029258" quat="0.697634 -0.115356 0.115356 0.697634" mass="0.048304" diaginertia="1.88038e-05 1.7493e-05 3.56779e-06" />
                                                <joint name="right_finger_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.85" />
                                                <geom name="j11" material="robot0:gripper_finger_mat" type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="3" contype="2" mesh="right_finger" />
                                            </body>
                                        </body>
                                        <body name="right_inner_knuckle" pos="0 -0.02 0.074098">
                                            <inertial pos="1.866e-06 -0.022047 0.026133" quat="0.66415 0.242702 -0.242721 0.664144" mass="0.023013" diaginertia="8.34209e-06 6.0949e-06 2.75601e-06" />
                                            <joint name="right_inner_knuckle_joint" pos="0 0 0" axis="-1 0 0" limited="true" range="0 0.85" />
                                            <geom type="mesh" rgba="0.753 0.753 0.753 1" conaffinity="1" contype="0" mesh="right_inner_knuckle" />
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>

        <!-- <camera mode="fixed" name="main_view" pos="1 0.75 1" quat="1 0 0 0" fovy="45"/> -->
    
    <!-- <site name="hand-goal-site" type="cylinder" pos="0 0.5 0" size="0.06 0.001" rgba="0.5 0.2 0.2 1"/> -->

		<body pos="1.3 0.75 0.2" name="table0">
			<geom name="table0" size="0.45 0.75 0.2" type="box" mass="2000" material="table_mat"></geom>
		</body>
        <body pos="1 0.75 0.4" name="table2">
			<geom name="smalltable" size="0.10 0.15 0.05" type="box" mass="2000" material="smalltable_mat"></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>

    <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="10.0"/>
        <!-- <position ctrllimited="true" ctrlrange="0 0.85" gear="1" kp="10" joint="left_inner_knuckle_joint"/> -->
    </actuator>

</mujoco>
