<!-- ======================================================
	Modified from one of the MuJoCo example XMl files.
    Oxford Robotics Institute 2017

	Model 		:: Jaco Arm from Kinova Robotics
 Source		: www.github.com/Kinovarobotics
 Downloaded	: July 25, 2015

	Mujoco		:: Advanced physics simulation engine
 Source		: www.roboti.us
 Version		: 1.50

	Authors		:: Sasha Salter, Dushyant Rao
 ====================================================== -->

<mujoco model="jaco(v1.5)">
    <compiler angle="radian" meshdir="meshes/" texturedir="textures/"/>

    <!-- option gravity="0 0 -2"/ -->
    <!-- <option timestep="0.1"/> -->

    <asset>
        <mesh file="jaco_link_base.stl"/>
        <mesh file="jaco_link_1.stl"/>
        <mesh file="jaco_link_2.stl"/>
        <mesh file="jaco_link_3.stl"/>
        <mesh file="jaco_link_4.stl"/>
        <mesh file="jaco_link_5.stl"/>
        <mesh file="jaco_link_hand.stl"/>
        <mesh file="jaco_link_finger_1.stl"/>
        <mesh file="jaco_link_finger_2.stl"/>
        <mesh file="jaco_link_finger_3.stl"/>

        <texture type="skybox" builtin="gradient" rgb1="1. 1. 1." rgb2="1. 1. 1."
        width="100" height="100"/>
        <texture name="groundplane" type="2d" builtin="checker" rgb1=".25 .26 .25"
        rgb2=".22 .22 .22" width="100" height="100" mark="none" markrgb=".8 .8 .8"/>
        <texture name="woodtabletop" type="cube" filefront="wooden.png" rgb1=".07 .07 .1"/> 

        <texture name="0" type="cube" filefront="Numbers-0y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="1" type="cube" filefront="Numbers-1y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="2" type="cube" filefront="Numbers-2y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="3" type="cube" filefront="Numbers-3y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="4" type="cube" filefront="Numbers-4y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="5" type="cube" filefront="Numbers-5y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="6" type="cube" filefront="Numbers-6y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="7" type="cube" filefront="Numbers-7y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="8" type="cube" filefront="Numbers-8y-Red-icon.png" rgb1=".07 .07 .1"/>
        <texture name="9" type="cube" filefront="Numbers-9y-Red-icon.png" rgb1=".07 .07 .1"/>

        <material name="num0" texture="0"/>
        <material name="num1" texture="1"/>
        <material name="num2" texture="2"/>
        <material name="num3" texture="3"/>
        <material name="num4" texture="4"/>
        <material name="num5" texture="5"/>
        <material name="num6" texture="6"/>
        <material name="num7" texture="7"/>
        <material name="num8" texture="8"/>
        <material name="num9" texture="9"/>

        <material name="wood" texture="woodtabletop" specular="0.1" shininess=".01" reflectance=".00001"/>
        <material name="MatSteel" specular="1" shininess="1" reflectance="1" rgba="0.3284 0.3338 0.3378 1"/>
        <material name="MatViz" specular="1" shininess=".1" reflectance="0.5" rgba=".07 .07 .1 1"/>
        <material name="MatGnd" texture="groundplane"  texrepeat="5 5" specular="1" shininess=".3" reflectance="0.00001"/>
    </asset>


    <visual>
        <map fogstart="3" fogend="5" force="0.1" znear="0.5"/>
        <quality shadowsize="0" offsamples="0"/>
        <global offwidth="800" offheight="800"/>
    </visual>

    <default>
        <geom margin="0.001"/>
        <joint limited="false" damping="0.2" armature=".01"/>

        <!--geom class for visual and collision geometry-->
        <default class ="vizNcoll">
            <geom material="MatViz" type="mesh" group="0"  contype="1" conaffinity="0"/>
            <joint pos="0 0 0" type="hinge"/>
        </default>
        <default class ="ground">
            <geom material="MatGnd" type="plane" contype="0" conaffinity="1"/>
        </default>
        <default class="border">
            <geom type="capsule" size="0.1" rgba=".4 .4 .4 1"/>
        </default>

        <default class="borderpost">
            <geom type="box" size="0.01 0.01 0.01" rgba=".55 .55 .55 1"/>
        </default>
    </default>

    <worldbody>
        <light directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular=".1 .1 .1" pos="1 1 1.5" dir="-1 -1 -1.3"/>
        <geom name="ground" class="ground" pos="0 0 0" size="2 2 1"/>

        <body name="pinpad" childclass="borderpost" pos="0.5 0 0.05">
            <geom type="box" size="0.22 0.285 0.0495"/>
            <body name="key0" pos="0 -0.21 0.04">
                <geom type="box" size="0.06 0.06 0.01"  material="num0"/>
            </body>
            <body name="key1" pos="0 0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01"  material="num5"/>
            </body>
            <body name="key2" pos="0 0.21 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num2"/>
            </body>                    
            <body name="key3" pos="0 -0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num8"/>
            </body>
            <body name="key4" pos="0.14 0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num6"/>
            </body>
             <body name="key5" pos="0.14 0.21 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num3"/>
            </body>
             <body name="key6" pos="0.14 -0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num9"/>
            </body>
             <body name="key7" pos="-0.14 0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num4"/>
            </body>
             <body name="key8" pos="-0.14 0.21 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num1"/>
            </body>     
              <body name="key9" pos="-0.14 -0.07 0.04">
                <geom type="box" size="0.06 0.06 0.01" material="num7"/>
            </body>
        </body>  

        <body name="base" childclass="vizNcoll" pos="0.5 0 0.00">
<!--            <joint type= "free"/>-->
            <geom type="box" size="0.40 0.40 0.05" material="wood"/>
        </body>

<!--         <geom class="border" fromto="0.27 0.23 0.005 0.73 0.23 0.005"  />
        <geom class="border" fromto="0.27 -0.23 0.005 0.73 -0.23 0.005"  />
        <geom class="border" fromto="0.73 0.23 0.005 0.73 -0.23 0.005"  />
        <geom class="border" fromto="0.27 0.23 0.005 0.27 -0.23 0.005"  />
        <geom class="borderpost" pos="0.73 0.23 0.005"/>
        <geom class="borderpost" pos="0.27 0.23 0.005"/>
        <geom class="borderpost" pos="0.73 -0.23 0.005"/>
        <geom class="borderpost" pos="0.27 -0.23 0.005"/> -->

        <body name="camera_one" pos="0.5 -0.8 2.5">
            <camera mode="targetbody" target="base" name="camera_one"/>
            <geom size="0.01 0.01 0.01"/>
        </body>
        <body name="camera_two" pos="0.2 -1 2">
            <camera mode="targetbody" target="base" name="camera_two"/>
            <geom size="0.01 0.01 0.01"/>
        </body>
        <body name="camera_three" pos="1 -1.3 2.5">
            <camera mode="targetbody" target="base" name="camera_three"/>
            <geom size="0.01 0.01 0.01"/>
        </body>
        <body name="camera_four" pos="0.8 -2 1.75">
            <camera mode="targetbody" target="base" name="camera_four"/>
            <geom size="0.01 0.01 0.01"/>
        </body>
        <body name="camera_main" pos="2.1 0.0 1.4">
			<camera mode="targetbody" target="base" name="camera_main"/>
			<geom size="0.01 0.01 0.01"/>
		</body> 

        <body name="jaco_link_base" childclass="vizNcoll" pos="0 0 0.25">
            <inertial pos="-3.14012e-008 0.000451919 0.0770704" quat="1 -0.000920048 6.51183e-006 5.62643e-005" mass="0.787504" diaginertia="0.00187445 0.00186316 0.000676952" />
            <geom mesh="jaco_link_base"/>
            <geom type="capsule" pos="0 0 -0.125" size=".045 0.125"/>


            <body name="jaco_link_1" childclass="vizNcoll" pos="0 0 0.1535" quat="0 0 1 0">
                <inertial pos="-1.14317e-006 0.0102141 -0.0637045" quat="0.702792 0.0778988 -0.0778994 0.702813" mass="0.613151" diaginertia="0.00149348 0.00144012 0.000401632" />
                <joint name="jaco_joint_1" axis="0 0 -1" frictionloss="3" />
                <geom mesh="jaco_link_1"/>


                <body name="jaco_link_2" pos="0 0 -0.1185" quat="0 0 0.707107 0.707107">
                    <inertial pos="0.206657 -1.55187e-007 -0.030675" quat="8.97411e-006 0.707331 5.06696e-006 0.706883" mass="1.85031" diaginertia="0.0402753 0.0398125 0.00121953" />
                    <joint name="jaco_joint_2" axis="0 0 1" frictionloss="100" />
                    <geom mesh="jaco_link_2"/>

                    <!-- Elbow -->
                    <body name="jaco_link_3" pos="0.41 0 0" quat="0 0.707107 0.707107 0">
                        <inertial pos="0.0847979 -1.18469e-007 -0.020283" quat="2.33799e-005 0.694869 1.96996e-005 0.719136" mass="0.70372" diaginertia="0.00386732 0.00373181 0.000358773" />
                        <joint name="jaco_joint_3" axis="0 0 -1" frictionloss="10" />
                        <geom mesh="jaco_link_3"/>

                        <!-- Upper wrist -->
                        <body name="jaco_link_4" pos="0.207 0 -0.01125" quat="0 0.707107 0 -0.707107">
                            <inertial pos="0.00914824 4.53141e-008 -0.0370941" quat="0.971237 -1.92989e-005 -0.238115 3.23646e-005" mass="0.227408" diaginertia="0.000176463 0.00017225 0.000108303" />
                            <joint name="jaco_joint_4" axis="0 0 -1" frictionloss="3" />
                            <geom mesh="jaco_link_4"/>

                            <!-- Lower wrist -->
                            <body name="jaco_link_5" pos="0.0343 0 -0.06588" quat="0.887011 0 -0.461749 0">
                                <inertial pos="0.00914824 4.53141e-008 -0.0370941" quat="0.971237 -1.92989e-005 -0.238115 3.23646e-005" mass="0.227408" diaginertia="0.000176463 0.00017225 0.000108303" />
                                <joint name="jaco_joint_5" axis="0 0 -1" frictionloss="3" />
                                <geom mesh="jaco_link_5"/>

                                <!-- Spinny hand -->
                                <body name="jaco_link_hand" pos="0.0343 0 -0.06588" quat="0.627211 -0.326506 -0.326506 0.627211">
                                    <inertial pos="0.0036132 -6.2241e-005 -0.0583749" quat="0.669114 -0.237618 -0.23799 0.66271" mass="0.58074" diaginertia="0.000817146 0.000671192 0.000606807" />
                                    <joint name="jaco_joint_6" axis="0 0 -1" frictionloss="3" />
                                    <geom mesh="jaco_link_hand" rgba=".13 .13 .13 1"/>


                                    <body name="jaco_link_finger_1" pos="-0.03978 0 -0.10071" quat="-0.414818 -0.329751 -0.663854 0.52772">
                                        <inertial pos="0.0485761 -0.000715511 2.09499e-008" quat="0.507589 0.507348 0.492543 0.492294" mass="0.0379077" diaginertia="4.00709e-005 4.00528e-005 2.156e-006" />
                                        <joint name="jaco_joint_finger_1" limited="true" axis="0 0 1" range="0 0.698132"/>
                                        <geom mesh="jaco_link_finger_1"/>
                                    </body>


                                    <body name="jaco_link_finger_2" pos="0.03569 -0.0216 -0.10071" quat="0.625248 -0.567602 0.434845 0.312735">
                                        <inertial pos="0.0485761 -0.000715511 2.09499e-008" quat="0.507589 0.507348 0.492543 0.492294" mass="0.0379077" diaginertia="4.00709e-005 4.00528e-005 2.156e-006" />
                                        <joint name="jaco_joint_finger_2" limited="true" axis="0 0 1" range="0 0.698132"/>
                                        <geom mesh="jaco_link_finger_2"/>
                                    </body>


                                    <body name="jaco_link_finger_3" pos="0.03569 0.0216 -0.10071" quat="0.561254 -0.620653 0.321748 0.443014">
                                        <inertial pos="0.0485761 -0.000715511 2.09499e-008" quat="0.507589 0.507348 0.492543 0.492294" mass="0.0379077" diaginertia="4.00709e-005 4.00528e-005 2.156e-006" />
                                        <joint name="jaco_joint_finger_3" limited="true" axis="0 0 1" range="0 0.698132"/>
                                        <geom mesh="jaco_link_finger_3"/>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </worldbody>

    <actuator>
        <!--  ================= Torque actuators (Weren't present in the URDF. Added seperately)================= /-->
        <motor joint='jaco_joint_1' 	name='Ajaco_joint_1' gear="50"/>
        <motor joint='jaco_joint_2' 	name='Ajaco_joint_2' gear="100"/>
        <motor joint='jaco_joint_3' 	name='Ajaco_joint_3' gear="100"/>
        <motor joint='jaco_joint_4' 	name='Ajaco_joint_4' gear="50"/>
        <motor joint='jaco_joint_5' 	name='Ajaco_joint_5' gear="50"/>
        <motor joint='jaco_joint_6' 	name='Ajaco_joint_6' gear="50"/>
        <motor joint='jaco_joint_finger_1' 	name='Ajaco_joint_finger_1' gear='10'/>
        <motor joint='jaco_joint_finger_2' 	name='Ajaco_joint_finger_2' gear='10'/>
        <motor joint='jaco_joint_finger_3' 	name='Ajaco_joint_finger_3' gear='10'/>
    </actuator>
</mujoco>
