<mujoco model="yumi">
    <compiler angle="radian" />
    <visual>
        <quality shadowsize="10000"/>
    </visual>
    <option timestep="0.01" gravity="0 0 -0.1" integrator="Euler" impratio="1" />
    <size njmax="500" nconmax="100" />

    <default>
        <default class="yumi_robot">
            <geom margin="0.001" rgba="1 1 1 1" solimp="0.99 0.999 0.01" solref="0.01 1" type="mesh" user="0"></geom>
            <joint armature="1" damping="50" frictionloss="0" stiffness="0"></joint>
            
            <default class="yumi_gripper">
                <geom condim="4" margin="0.001" type="box" user="0" rgba="0.356 0.361 0.376 1.0"></geom>
                <joint armature="100" damping="500" limited="true" solimplimit="0.99 0.999 0.01" solreflimit="0.001 1" type="slide" ></joint>
            </default>
        </default>
        <default class="door">
            <geom condim="4" margin="0.001" type="box" user="0" rgba="0.356 0.361 0.376 1.0"></geom>
            <joint armature="1" damping="500" limited="true" solimplimit="0.99 0.999 0.01" solreflimit="0.01 1" type="slide" ></joint>
        </default>

    </default>


    <contact>
        <exclude body1="gripper_r_finger_r" body2="gripper_r_finger_l"></exclude>
        <exclude body1="gripper_r_base" body2="yumi_link_7_r"></exclude>
    </contact>
    <asset>
        <mesh name="coarse_body" file="coarse_body.stl" />
        <mesh name="coarse_link_1" file="coarse_link_1.stl" />
        <mesh name="coarse_link_2" file="coarse_link_2.stl" />
        <mesh name="coarse_link_3" file="coarse_link_3.stl" />
        <mesh name="coarse_link_4" file="coarse_link_4.stl" />
        <mesh name="coarse_link_5" file="coarse_link_5.stl" />
        <mesh name="coarse_link_6" file="coarse_link_6.stl" />
        <mesh name="coarse_link_7" file="coarse_link_7.stl" />
        <mesh name="base" file="base.stl" />
        <mesh name="finger" file="finger.stl" />
        <!-- for object modeling -->
        <material name="red" rgba="1 0.3 0.28 1"/>
    </asset>
    <worldbody>
        <camera name="extra_1" mode="fixed" pos="2.0 0 0.2" euler="1.57 1.57 0"/>
        <camera name="extra_2" mode="fixed" pos="0.7 0.7 0.2" euler="1.57 2.3 0"/>
        <light diffuse=".5 .5 .5" pos="2 -2 2" dir="-1  1 -1"/>
        <light diffuse=".5 .5 .5" pos="2  2 2" dir="-1 -1 -1"/>
        		
		<body pos="0 0 -0.01" name="table0">
			<geom size="0.4 0.5 0.01" type="box" mass="2000" friction="1.5 1.5 1.5"></geom>
		</body>
        <!-- <geom rgba="1 1 1 1" name="table" type="box" pos="0 0 -0.01" size="0.4 0.5 0.01" condim="4" friction="1.5 1.5 1.5"/> -->
        <!-- <geom name="rod" type="cylinder" pos="0 0 0.5" size=".01 .1" density="100"/> --> 
        
        <body childclass="yumi_robot" name="yumi_base_link" pos="-0.4 0 0">
            <inertial pos="0 0 0" mass="0" diaginertia="0 0 0" />
            <body name="yumi_body" pos="0 0 0">
                <inertial pos="0 0 0" quat="0.707107 0 0 0.707107" mass="15" diaginertia="0.6 0.5 0.3" />
                <geom type="mesh" rgba="0.8 0.8 0.8 1" mesh="coarse_body"/>
                <body name="yumi_link_1_r" pos="0.05355 -0.0725 0.41492" quat="0.219255 -0.407796 0.31429 -0.828765">
                    <inertial pos="0 -0.03 0.12" mass="2" diaginertia="0.1 0.1 0.1" />
                    <joint type="hinge" name="yumi_joint_1_r" pos="0 0 0" axis="0 0 1" limited="true" range="-2.94088 2.94088" />
                    <geom type="mesh" mesh="coarse_link_1"/>
                    <body name="yumi_link_2_r" pos="0.03 0 0.1" quat="0.707107 0.707107 0 0">
                        <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                        <joint type="hinge" name="yumi_joint_2_r" pos="0 0 0" axis="0 0 1" limited="true" range="-2.50455 0.759218" />
                        <geom type="mesh" mesh="coarse_link_2" />
                        <body name="yumi_link_3_r" pos="-0.03 0.17283 0" quat="0.707107 -0.707107 0 0">
                            <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                            <joint type="hinge" name="yumi_joint_7_r" pos="0 0 0" axis="0 0 1" limited="true" range="-2.94088 2.94088" />
                            <geom type="mesh" mesh="coarse_link_3" />
                            <body name="yumi_link_4_r" pos="-0.04188 0 0.07873" quat="0.5 0.5 -0.5 0.5">
                                <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                                <joint type="hinge" name="yumi_joint_3_r" pos="0 0 0" axis="0 0 1" limited="true" range="-2.15548 1.39626" />
                                <geom type="mesh" mesh="coarse_link_4" />
                                <body name="yumi_link_5_r" pos="0.0405 0.16461 0" quat="0.707107 -0.707107 0 0">
                                    <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                                    <joint type="hinge" name="yumi_joint_4_r" pos="0 0 0" axis="0 0 1" limited="true" range="-5.06145 5.06145" />
                                    <geom type="mesh" mesh="coarse_link_5" />
                                    <body name="yumi_link_6_r" pos="-0.027 0 0.10039" quat="0.707107 0.707107 0 0">
                                        <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                                        <joint type="hinge" name="yumi_joint_5_r" pos="0 0 0" axis="0 0 1" limited="true" range="-1.53589 2.40855" />
                                        <geom type="mesh" mesh="coarse_link_6" />
                                        <body name="yumi_link_7_r" pos="0.027 0.029 0" quat="0.707107 -0.707107 0 0">
                                            <inertial pos="0 0 0" mass="2" diaginertia="0.1 0.1 0.1" />
                                            <joint type="hinge" name="yumi_joint_6_r" pos="0 0 0" axis="0 0 1" limited="true" range="-3.9968 3.9968" />
                                            <geom type="mesh" mesh="coarse_link_7" />
                                            <body name="gripper_r_base" pos="0 0 0.007" quat="0 0 0 1">
                                                <inertial pos="0 0 0" mass="0.2" diaginertia="0.0001 0.0001 0.0001" />
                                                <geom type="mesh" rgba="0.8 0.8 0.8 1" mesh="base" />
                                                <body childclass="yumi_gripper" name="gripper_r_finger_r" pos="-0.002 -0.0 0.0837" >
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="1e-06 1e-06 1e-06" />
                                                    <joint name="gripper_r_joint" pos="0 0 0" axis="0.96592582628 0 -0.2588190451" type="slide" limited="true" range="-0.0243 -0.003 "/>
                                                    <!-- <geom type="mesh" rgba="0.8 0.8 0.8 1" mesh="finger" condim="4" friction="1 0.05 0.01"/> -->
                                                    <geom rgba="1 0.4 0 1" type="box" pos="0.00 0.00 0.012" size="0.005 0.007 0.04" quat="0 0.0261769 0 0.9996573" condim="4" friction="1.5 1.5 1.5" density="100"/>
                                                    <!-- <geom rgba="0.8 0.8 0.8 1" type="box" pos="0.0104 0.00 0.037" size="0.0052 0.001 0.0028" quat="0 0.1305262 0 0.9914449" condim="4" friction="1.5 1.5 1.5" density="100"/> -->
                                                </body>
                                                <body childclass="yumi_gripper" name="gripper_r_finger_l" pos="0.002 0.0 0.0837" quat="0 0 0 1">
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="1e-06 1e-06 1e-06" />
                                                    <joint name="gripper_r_joint_m" pos="0 0 0" axis="0.96592582628 0 -0.2588190451" type="slide" limited="true" range="-0.0243 -0.003" />
                                                    <!-- <geom type="mesh" rgba="0.8 0.8 0.8 1" mesh="finger" condim="4" friction="1 0.05 0.01"/> -->
                                                    <geom rgba="1 0.4 0 1" type="box" pos="0.00 0.00 0.012" size="0.005 0.007 0.04" quat="0 0.0261769 0 0.9996573"  condim="4" friction="1.5 1.5 1.5" density="100"/>
                                                    <geom rgba="1 0.4 0 1" type="box" pos="0.005 0.00 0.047" size="0.0052 0.007 0.005" quat="0 0.0 0 0.1" condim="4" friction="1.5 1.5 1.5" density="100"/>
                                                </body>
                                            </body>
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body> 


        <body name="goal" pos="0 0 0">
            <site name="goal" pos="0 0 0" size="0.04 0.04 0.02" rgba="1 0 0 0" type="sphere"></site>
            <!-- <geom rgba="1 0 0 1" type="sphere" size="0.04 0.001 0.04" density='0.000001'/> -->
            <!-- <joint name="goal_slidex" type="slide" pos="0 0 0" axis="1 0 0" range="-10.3213 10.3" damping="1.0"/>
            <joint name="goal_slidey" type="slide" pos="0 0 0" axis="0 1 0" range="-10.3213 10.3" damping="1.0"/>
            <joint name="goal_slidez" type="slide" pos="0 0 0" axis="0 0 1" range="-10.3213 10.3" damping="1.0"/> -->
        </body>

        <body childclass="door" name="cabinet" pos="0.02 0 0.05" quat="0.5 0.5 0.5 0.5">
	        <body name="door" pos="0 0 0" quat="0 -0.7071068 0 0.7071068">
	            <joint name="door_l_joint" pos="0 -0.025 0.05" axis="1 0 0" type="hinge" limited="true" range="-1.52 0" damping="0.1" armature="0" />
	            <geom type="box" pos="0.1 0 0" size="0.01 0.08 0.12" rgba=".1 .1 .9 1" quat="0.5 0.5 -0.5 -0.5" friction="1.5" mass="1"/>
	           	<geom type="cylinder" quat="0 -0.7071068 0 0.7071068" pos="0.1 -0.040 -0.025" size="0.005 0.045 0.005" rgba="0 0 0 1" friction="1.5" mass="1"/>
	           	<geom type="box" pos="0.15 -0.020 -0.025" size="0.005 0.025 0.005" rgba="0 0 0 1" friction="1.5" mass="1"/>                   
	        </body>
        </body>

        <!--
        <body name="door" pos="0 0.725 0.1">
                <joint name="doorjoint" pos=".21 0 0" axis="0 0 1"
                       type="hinge" limited="true" range="0 1.0472" damping="0.1" armature="0" />

                <geom material="wood" type="box" pos="0 0 .0" size="0.20 0.01 0.20"
                      contype="2" conaffinity="2" />
                <geom material="wood" type="box" pos="0 0 .0" size="0.20 0.01 0.20"
                      contype="2" conaffinity="2" />
                <geom type="box" pos="0 0 .21" size="0.2 0.01 0.01"
                      material="darkwood" contype="0" conaffinity="0" />
                <geom type="box" pos="0 0 -.21" size="0.2 0.01 0.01"
                      material="darkwood" contype="0" conaffinity="0" />

                <geom type="box" pos="0.21 0 0" size="0.01 0.01 0.22"
                      material="darkwood" contype="0" conaffinity="0" />
                <geom type="box" pos="-0.21 0 0" size="0.01 0.01 0.22"
                      material="darkwood" contype="0" conaffinity="0" />

                <geom type="box" pos="-.05 -0.06 0.05" size="0.1085 0.01 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2" />
                <geom type="box" pos="0.05 -0.03 0.05" size="0.01 0.03 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2"/>
                <geom type="box" pos="-0.15 -0.03 0.05" size="0.01 0.03 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2"/>
        </body>
        -->
	<!-- door before simplication
	     <body name="door_l" pos="0.5 0 0.3" quat="-0.7071068 0 0 0.7071068">
                <joint name="doorjoint" pos=".21 0 0" axis="0 0 1"
                       type="hinge" limited="true" range="0 1.0472" damping="0.1" armature="0" />

rgba.01 0.20"
rgba
rgba
                <geom type="box" pos="0 0 .0" size="0.20 0.01 0.20"
                    contype="2" conaffinity="2" />
            
                <geom type="box" pos="0 0 .21" size="0.2 0.01 0.01"
                    contype="0" conaffinity="0" />
                <geom type="box" pos="0 0 -.21" size="0.2 0.01 0.01"
                    contype="0" conaffinity="0" />

                <geom type="box" pos="0.21 0 0" size="0.01 0.01 0.22"
                    contype="0" conaffinity="0" />
                <geom type="box" pos="-0.21 0 0" size="0.0rgba1 0.01 0.22"
                    contype="0" conaffinity="0" />rgba
rgba
                <geom type="box" pos="-.025 -0.06 0.05" sirgbaze="0.05 0.01 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2" />
                <geom type="box" pos="0.05 -0.03 0.05" sizrgbae="0.01 0.03 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2"/>
                <geom type="box" pos="-0.1 -0.03 0.05" sizrgbae="0.01 0.03 0.01" rgba="0.2 0.2 0.2 1" contype="2" conaffinity="2"/>
		</body>
	-->
  




		<!-- <body name="tool" pos="0.04846357 0.00586877 0.0">
			<joint name="object0:joint" type="free" damping="0.01"></joint>
			<geom size="0.008 0.008 0.008" type="box" condim="3" name="object0" mass="1"></geom>
			<site name="object0" pos="0 0 0" size="0.005 0.005 0.005" rgba="1 0 0 1" type="sphere"></site>
		</body> -->

        <!-- <body name="rod" pos="0 0 0">
            <geom name="rod" type="cylinder" pos="0.2 0 0.5" material="red" size=".01 .1" density="100"/>
            <inertial diaginertia="0.000180024 0.000180024 0.000343383" mass="0.2" pos="0 0 0"/>
            <joint limited="false" name="puckjoint" type="free"/>
        </body>   -->
        
        <body mocap="true" name="mocap" pos="0.16846357 0.00586877 0.41461996">
            <!--For debugging, set the alpha to 1-->
            <!--<geom conaffinity="0" contype="0" pos="0 0 0" rgba="0.5 0.5 0.5 1" size="0.1 0.02 0.02" type="box"></geom>-->
            <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0.0 1 0 1" size="0.04 0.001 0.04" type="sphere"></geom>
            <site pos="0 0 0" rgba="0.0 0.5 0.5 0" size="0.01" type="sphere"></site>
        </body>

    </worldbody>

    <actuator>
        <!-- Don't change ranges here, change them in __init__ of YumiReacher -->
        <motor joint="yumi_joint_1_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_2_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_7_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_3_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_4_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_5_r" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="yumi_joint_6_r" ctrlrange="-100.0 100.0" ctrllimited="true"/> 
       
        <motor joint="gripper_r_joint" ctrlrange="-100.0 100.0" ctrllimited="true"/>
        <motor joint="gripper_r_joint_m" ctrlrange="-100.0 100.0" ctrllimited="true"/>
               
        <!--velocity joint="yumi_joint_1_l" ctrlrange="-200.0 200.0" ctrllimited="true" kv="100"/>
        <velocity joint="yumi_joint_2_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="100"/>
        <velocity joint="yumi_joint_7_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="60"/>
        <velocity joint="yumi_joint_3_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="30"/>
        <velocity joint="yumi_joint_4_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="30"/>
        <velocity joint="yumi_joint_5_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="30"/>
        <velocity joint="yumi_joint_6_l" ctrlrange="-20.0 20.0" ctrllimited="true" kv="30"/-->
    </actuator>


    <equality>
        <weld body1="mocap" body2="gripper_r_base" solref="0.2 1"></weld>
    </equality>

</mujoco>
