<mujocoinclude>

    <camera pos="0 0 3.5" name="topview" />
    <camera pos="0.6 -0.6 2" mode="fixed" xyaxes="0.1 0.1 0 -0.1 0.1 0.1" name="sideview" />

    <body name="base_link" pos="0 0.4 0.82" quat="-0.707107 0 0 0.707107" >
        <!-- The mass 4 kg of base link might be incorrect (it is for ur5) -->
        <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="4" diaginertia="0.0072 0.00443333 0.00443333" />
        <geom mesh="C_base" class="D_robot_collision" />
        <geom mesh="V_base" class="D_robot_visual" />

        <body name="shoulder_link" pos="0 0 0.163">
            <inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" />
            <geom mesh="C_shoulder" class="D_robot_collision" />
            <geom mesh="V_shoulder" class="D_robot_visual" />
            <joint name="shoulder_pan_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" />

            <body name="upper_arm_link" pos="0 0.138 0" quat="0.707107 0 0.707107 0">
                <inertial pos="0 0 0.2125" mass="8.393" diaginertia="0.133886 0.133886 0.0151074" />
                <geom mesh="C_upperarm" class="D_robot_collision" />
                <geom mesh="V_upperarm" class="D_robot_visual" />
                <joint name="shoulder_lift_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-6.28319 6.28319" />

                <body name="forearm_link" pos="0 -0.131 0.425">
                    <inertial pos="0 0 0.196" mass="2.275" diaginertia="0.0311796 0.0311796 0.004095" />
                    <geom mesh="C_forearm" class="D_robot_collision" />
                    <geom mesh="V_forearm" class="D_robot_visual" />
                    <joint name="elbow_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" />

                    <body name="wrist_1_link" pos="0 0 0.392" quat="0.707107 0 0.707107 0">
                        <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="1.219" diaginertia="0.21942 0.111173 0.111173" />
                        <geom mesh="C_wrist1" class="D_robot_collision" />
                        <geom mesh="V_wrist1" class="D_robot_visual" />
                        <joint name="wrist_1_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-6.28319 6.28319" />

                        <body name="wrist_2_link" pos="0 0.127 0">
                            <inertial pos="0 0 0.1" mass="1.219" diaginertia="0.0025599 0.0025599 0.0021942" />
                            <geom mesh="C_wrist2" class="D_robot_collision" />
                            <geom mesh="V_wrist2" class="D_robot_visual" />
                            <joint name="wrist_2_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" />

                            <body name="wrist_3_link" pos="0 0 0.1">
                                <inertial pos="0 0.0771683 0" quat="0.707107 0 0 0.707107" mass="0.1889" diaginertia="0.000132134 9.90863e-05 9.90863e-05" />
                                <geom mesh="C_wrist3" class="D_robot_collision" />
                                <geom mesh="V_wrist3" class="D_robot_visual" />
                                <joint name="wrist_3_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-6.28319 6.28319" />

                                <body name="ee_link" pos="0 0.1 0" quat="0.707107 -0.707107 0 0">

                                    <inertial pos="0 0.0823 0" quat="0.707107 -0.707107 0 0" mass="0" diaginertia="0 0 0" />

                                    <body name="robotiq_85_adapter_link" pos="0.02 0 0" quat="0 0.709 0 0.706">
                                        <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                        <geom quat="0.500048 0.5 0.499952 0.5" type="mesh" group="0" mesh="robotiq_85_gripper_adapter_plate" />
                                        <geom quat="0.500048 0.5 0.499952 0.5" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_adapter_plate" />
                                        <body name="robotiq_85_base_link" pos="0.0085 0 -0.0041">
                                            <body name="hand" pos="0.1 0 0"/>
                                            <site name="hand-goal-site" pos="0.1 0 0" size="0.01" rgba="1 0 0 1" type="sphere" group="1"/>
                                            <!--  <site name="grip_site_cylinder" pos="-0.02 0 0" quat="0.707 0 0.707 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1" />-->
                                            <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                            <geom quat="3.27596e-05 0.707141 0.707073 3.27564e-05" type="mesh" group="0" mesh="robotiq_85_gripper_base" />
                                            <geom quat="3.27596e-05 0.707141 0.707073 3.27564e-05" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_base" />
                                            <body name="robotiq_85_left_knuckle_link" pos="0.04191 0.0306 0" quat="0.500048 -0.5 0.5 0.499952">
                                                <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                <joint name="robotiq_85_left_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.8575" />
                                                <geom pos="0 0.04191 -0.0306" quat="9.38231e-07 0.707141 -9.3814e-07 0.707073" type="mesh" group="0" mesh="robotiq_85_gripper_joint_0_L" name="robotiq_85_gripper_joint_0_L"/>
                                                <geom pos="0 0.04191 -0.0306" quat="9.38231e-07 0.707141 -9.3814e-07 0.707073" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_0_L" />
                                                <body name="robotiq_85_left_finger_link" pos="0 0.00508 0.03134" quat="4.63268e-05 1 0 0">
                                                    <!--  <geom pos="0 0.047 -0.008" size="0.005" type="sphere" rgba="1 0 0 1"/> -->
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                    <geom pos="0 -0.03683 0.06194" quat="0.707141 0 0.707073 0" type="mesh" group="0" mesh="robotiq_85_gripper_joint_1_L" name="robotiq_85_gripper_joint_1_L"/>
                                                    <geom pos="0 -0.03683 0.06194" quat="0.707141 0 0.707073 0" type="mesh" group="1" contype="0" conaffinity="0" mesh="robotiq_85_gripper_joint_1_L" />
                                                </body>
                                            </body>
                                            <body name="robotiq_85_right_knuckle_link" pos="0.04191 -0.0306 0" quat="0.500048 0.5 -0.5 0.499952">
                                                <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                <joint name="robotiq_85_right_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 1.5707" />
                                                <geom pos="0 0.04191 -0.0306" quat="9.38231e-07 0.707141 9.3814e-07 -0.707073" type="mesh" group="0" mesh="robotiq_85_gripper_joint_0_R" name="robotiq_85_gripper_joint_0_R"/>
                                                <geom pos="0 0.04191 -0.0306" quat="9.38231e-07 0.707141 9.3814e-07 -0.707073" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_0_R" />
                                                <body name="robotiq_85_right_finger_link" pos="0 0.00508 0.03134" quat="4.63268e-05 1 0 0">
                                                    <!-- <geom pos="0 0.047 -0.008" size="0.005" type="sphere" rgba="1 0 0 1"/> -->
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                    <geom pos="0 -0.03683 0.06194" quat="0.707141 0 -0.707073 0" type="mesh" group="0" mesh="robotiq_85_gripper_joint_1_R" name="robotiq_85_gripper_joint_1_R"/>
                                                    <geom pos="0 -0.03683 0.06194" quat="0.707141 0 -0.707073 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_1_R" />
                                                </body>
                                            </body>
                                            <body name="robotiq_85_left_inner_knuckle_link" pos="0.04843 0.0127 0" quat="0.500048 0.5 0.5 -0.499952">
                                                <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                <joint name="robotiq_85_left_inner_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 1.5707" />
                                                <geom pos="0 -0.04843 0.0127" quat="0.707141 0 0.707073 0" type="mesh" mesh="robotiq_85_gripper_joint_2_L" name="robotiq_85_gripper_joint_2_L"/>
                                                <geom pos="0 -0.04843 0.0127" quat="0.707141 0 0.707073 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_2_L" />
                                                <body name="robotiq_85_left_finger_tip_link" pos="0 0.04196 -0.0388">
                                                    <!-- <geom pos="0 -0.0065 -0.0185" size="0.005" type="sphere" rgba="0 1 0 1"/> -->
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                    <joint name="robotiq_85_left_finger_tip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 1.5707" />
                                                    <geom pos="0 -0.090381 0.051505" quat="0.707141 0 0.707073 0" type="mesh" group="0" mesh="robotiq_85_gripper_joint_3_L" name="robotiq_85_gripper_joint_3_L"/>
                                                    <geom pos="0 -0.090381 0.051505" quat="0.707141 0 0.707073 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_3_L" />
                                                </body>
                                            </body>
                                            <body name="robotiq_85_right_inner_knuckle_link" pos="0.04843 -0.0127 0" quat="0.500048 -0.5 -0.5 -0.499952">
                                                <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                <joint name="robotiq_85_right_inner_knuckle_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 1.5707" />
                                                <geom pos="0 -0.04843 0.0127" quat="0.707141 0 -0.707073 0" type="mesh" group="0" mesh="robotiq_85_gripper_joint_2_R" name="robotiq_85_gripper_joint_2_R"/>
                                                <geom pos="0 -0.04843 0.0127" quat="0.707141 0 -0.707073 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_2_R" />
                                                <body name="robotiq_85_right_finger_tip_link" pos="0 0.04196 -0.0388">
                                                    <inertial pos="0 0 0" mass="0.01" diaginertia="0.001 0.001 0.001" />
                                                    <!-- <geom pos="0 -0.0065 -0.0185" size="0.005" type="sphere" rgba="0 1 0 1"/> -->
                                                    <joint name="robotiq_85_right_finger_tip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="0 1.5707" />
                                                    <geom pos="0 -0.090381 0.051505" quat="0.707141 0 -0.707073 0" type="mesh" group="0"  mesh="robotiq_85_gripper_joint_3_R" name="robotiq_85_gripper_joint_3_R"/>
                                                    <geom pos="0 -0.090381 0.051505" quat="0.707141 0 -0.707073 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="robotiq_85_gripper_joint_3_R" />
                                                </body>
                                            </body>
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </body>

    <body mocap="true" name="mocap" pos="0 0.4 0.82">
        <!--For debugging, set the alpha to 1-->
        <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0.0 0.5 0.5 0.3" size="0.03" type="sphere" />
<!--        <site pos="0 0 0" rgba="0.8 0.5 0.5 1" size="0.01" type="sphere" />-->
    </body>

</mujocoinclude>