<mujocoinclude>
    <body name="robot" pos="0.0 0.0 0.0">
        <geom pos="0 0 -0.28" type="mesh" rgba="0.15 0.15 0.15 1" mesh="body-collision" />
        <geom pos="0 0 -0.28" type="mesh" rgba="0.15 0.15 0.15 1" mesh="top" />
        <geom size="0.1 0.2295" pos="0.28 0 0.2295" quat="1 0.0 0 0" type="cylinder" rgba="0.9 0.9 0.9 1" />
        <geom pos="0.28 0 0.456" quat="0.681998 0 0 -0.731354" type="mesh" rgba="0.7 0.7 0.7 1" mesh="ur5_base" />
        <geom size="0.2 0.3 0.3" pos="-0.2 0 0.3" type="box"/>

        <joint axis="1 0 0" damping="0." name="claw_free_1" pos="0 0 0" type="slide" range="-100 100"/>
        <joint axis="0 1 0" damping="0." name="claw_free_2" pos="0 0 0" type="slide" range="-100 100"/>
        <joint axis="0 0 1" damping="0." name="claw_free_3" pos="0 0 0" type="hinge" range="-100 100"/>

        <body name="base" pos="0 0 -0.2918">
            <geom type="box" name="base" mass="90" friction="0.4 0.4 0.4" size="0.2 0.2 0.01425" rgba="0. 0. 0. 1" contype="1" conaffinity="1"/>
        </body>

		<body name="base_link" pos="0.28 0 0.545159" quat="-0.03489996  0.          0.         -0.99939078">
            <inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" />
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="shoulder" />
            <body name="shoulder_link" pos="0 0.13585 0" quat=" 0.73727628  0.         -0.67559182  0.        ">
                <inertial pos="0 0 0.28" mass="8.393" diaginertia="0.226891 0.226891 0.0151074" />
                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="upperarm" />
                <body name="elbow_link" pos="0 -0.1197 0.425" quat="0.99904822  0.         -0.04361939  0.        ">
                    <inertial pos="0 0 0.25" mass="2.275" diaginertia="0.0494433 0.0494433 0.004095" />
                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="forearm" />
                    <body name="wrist_1_link" pos="0 0 0.39225" 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 type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist1" />

                        <body name="wrist_2_link" pos="0 0.093 0" quat="0.70710678  0.          0.         -0.70710678">
                            <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 type="mesh" rgba="0.7 0.7 0.7 1" friction="0.4 0.4 0.4" mesh="wrist2" />

                            <body name="wrist_3_link" pos="0 0 0.09465" quat="0 0 -1 0">
                                <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5"  mass="0.1879" diaginertia="0.033822 0.0171365 0.0171365" />
                                <geom type="mesh" rgba="0.7 0.7 0.7 1" friction="0.8 0.8 0.8" mesh="wrist3" />

                                <body name="ee_link" pos="0 0.0823 0" quat="0.707107 0 0 0.707107">
                                    <inertial pos="-0.01 0 0" mass="0.001"  diaginertia="1.66667e-08 1.66667e-08 1.66667e-08" />
                                    <geom size="0.005 0.005 0.005" friction="0.8 0.8 0.8" pos="-0.01 0 0" type="box" />

                                    <body name="robotiq_85_base_link" pos="0 0 0" quat="0.5 0.5 0.5 0.5 ">
                                        <inertial pos="0.00021987 -2.3546e-10 0.030163"  quat="0.707058 0.00723369 0.00723345 0.707081" mass="0.30915" diaginertia="0.00030737 0.000289758 0.000199102" />
                                        <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="robotiq_85_base_link_coarse" />

                                        <body name="left_outer_knuckle" pos="0.0306011 0 0.062792">
                                            <inertial pos="0.0178332 3.68915e-11 0.00214425"  quat="0.463241 0.534235 0.534235 0.463241" mass="0.00684839" diaginertia="1.38892e-06 1.28691e-06 2.45951e-07" />
                                            <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.2 0. 0.4" mesh="outer_knuckle_coarse" />
                                            <body name="left_outer_finger" pos="0.031691 0 -0.00193396">
                                                <inertial pos="0.00131452 0 0.0215404"  quat="0.999536 0 -0.0304703 0" mass="0.0273094" diaginertia="8.53972e-06 6.91333e-06 2.22664e-06" />
                                                <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="outer_finger_coarse" />
                                            </body>
                                        </body>

                                        <body name="left_inner_knuckle" pos="0.0127 0 0.0693075">
                                            <inertial pos="0.0170975 -3.93809e-10 0.0224918"   quat="0.947655 0 -0.319297 0" mass="0.0110931" diaginertia="5.59353e-06 3.96549e-06 1.88108e-06" />
                                            <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="inner_knuckle_coarse" />

                                            <body name="left_inner_finger" pos="0.0345853 0 0.045497" >
                                                <inertial pos="0.00602756 0 0.00669926"  quat="0.699065 0.106339 0.106339 0.699065" mass="0.00724255" diaginertia="1.70064e-06 1.58577e-06 3.69621e-07" />
                                                <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="inner_finger_coarse" />
                                            </body>
                                        </body>

                                        <body name="right_inner_knuckle" pos="-0.0127 0 0.0693075" quat="0 0 0 1">
                                            <inertial pos="0.0168606 3.93808e-10 0.02267"  quat="0.949317 0 -0.314322 0" mass="0.0110931" diaginertia="5.59353e-06 3.96549e-06 1.88108e-06" />
                                            <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="inner_knuckle_coarse" />
                                            <body name="right_inner_finger" pos="0.034106 0 0.0458574">
                                                <inertial pos="0.00602756 0 0.00669926"  quat="0.699065 0.106339 0.106339 0.699065" mass="0.00724255" diaginertia="1.70064e-06 1.58577e-06 3.69621e-07" />
                                                <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="inner_finger_coarse" />
                                            </body>
                                        </body>

                                        <body name="right_outer_knuckle" pos="-0.0306011 0 0.062792" quat="0 0 0 1">
                                            <inertial pos="0.0178097 3.60493e-11 0.00233123"  quat="0.460432 0.536658 0.536658 0.460432" mass="0.00684839" diaginertia="1.38892e-06 1.28691e-06 2.45951e-07" />
                                            <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="outer_knuckle_coarse" />
                                            <body name="right_outer_finger" pos="0.0317096 0 -0.00160136">
                                                <inertial pos="0.00108844 0 0.021553"  quat="0.999682 0 -0.0252262 0" mass="0.0273094" diaginertia="8.53972e-06 6.91333e-06 2.22664e-06" />
                                                <geom type="mesh" rgba="0.1 0.1 0.1 1" friction="0.8 0.8 0.8" mesh="outer_finger_coarse" />
                                            </body>
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </body>
</mujocoinclude>
