<!-- ======================================================
    This file is part of MuJoCo.
    Copyright 2009-2016 Roboti LLC.

    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.31
        Released     : 23Apr16

    Author        :: Vikash Kumar
        Contacts     : kumar@roboti.us
        Last edits     : 30Apr16, 30Nov15, 26Sept'15, 26July'15
====================================================== -->

<mujoco model="jaco(v1.31)">
    <compiler angle="radian" meshdir="meshes/" inertiafromgeom="true"/>

    <option impratio="2"/>

    <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 name="skybox" type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0" width="800" height="800" mark="random" markrgb="1 1 1"/>
        <texture name="groundplane" type="2d" builtin="checker" rgb1=".11 .11 .11" rgb2=".55 .55 .55" width="100" height="100" mark="none" markrgb=".8 .8 .8"/>

        <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>

   <default>
        <geom margin="0.001" contype="1" conaffinity="1"/>
        <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>

    <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" solref="0.02 0.1"/>

        <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" name="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" />
                <geom mesh="jaco_link_1" name="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" />
                    <geom mesh="jaco_link_2" name="jaco_link_2"/>


                    <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" />
                        <geom mesh="jaco_link_3" name="jaco_link_3"/>


                        <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" />
                            <geom mesh="jaco_link_4" name="jaco_link_4"/>


                            <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" />
                                <geom mesh="jaco_link_5" name="jaco_link_5"/>


                                <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" />
                                    <geom mesh="jaco_link_hand"  name="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" name="jaco_link_finger_1"  contype="1" conaffinity="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" name="jaco_link_finger_2" contype="1" conaffinity="1"/>
                                    </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" name="jaco_link_finger_3" contype="1" conaffinity="1"/>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>

        <!-- box -->
        <body name="box" pos="0.4 0.3 .04" euler="0 0 0">
            <inertial pos="0 0 0" mass="0.1"/>
            <geom name="box" pos="0 0 0" contype="1" conaffinity="1" rgba="0.8 0.2 0.2 1" size=".04" type="sphere" solref="0.02 0.1" friction="5 0.025 0.0005"/>
            <joint type="free" armature="0" damping="0" limited="false" stiffness="0"/>
        </body>

        <!-- release point -->
        <body name="release_point" pos="0.1 0.1 0.7" euler="0 0 0">
            <geom name="release_point_geom" pos="0 0 0" contype="0" conaffinity="0" rgba="0.6 0.3 0.3 0.3" size=".1" type="sphere"/>
        </body>

        <!-- max height -->
        <body name="max_point" pos="0.1 0.1 2.0" euler="0 0 0">
            <geom name="max_point_geom" pos="0 0 0" contype="0" conaffinity="0" rgba="0.6 0.3 0.3 0.3" size=".1" type="sphere"/>
        </body>

         <!-- target -->
        <body name="target" pos="0.4 2 1" euler="0 0 0">
            <geom name="target_geom" pos="0 0 0" contype="0" conaffinity="0" rgba="0.6 0.3 0.3 0.5" size=".2" type="sphere"/>
        </body>

        <!-- wall -->
        <body name="wall" pos="0 2.1 1" euler="0 0 0">
            <geom name="wall_geom" pos="0 0 0" contype="1" conaffinity="1" rgba="0.8 0.8 0.8 5" size="2 0.1 1" type="box" solref="0.02 0.1"/>
        </body>
    </worldbody>


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