<mujoco>

	<body mocap="true" name="gripper_r:mocap" pos="0 0 0" >
		<geom conaffinity="0" contype="0" pos="0 0. 0" rgba="0.0 0.5 0.0 0.5" size="0.005 0.005 0.005" type="box" group="1"></geom>
		<geom conaffinity="0" contype="0" pos="0 0. 0" rgba="1.0 0.0 0.0 0.5" size="0.15 0.005 0.005" type="box" group="1"></geom>
		<geom conaffinity="0" contype="0" pos="0 0. 0" rgba="0.0 1.0 0.0 0.5" size="0.005 0.15 0.005" type="box" group="1"></geom>
		<geom conaffinity="0" contype="0" pos="0 0. 0" rgba="0.0 0.0 1.0 0.5" size="0.005 0.005 0.15" type="box" group="1"></geom>
	</body>
	<body mocap="true" name="gripper_l:mocap" pos="0 0 0" >
		<geom conaffinity="0" contype="0" pos="0 0.1 0" rgba="0.0 0.5 0.0 0.5" size="0.005 0.005 0.005" type="box"></geom>
		<geom conaffinity="0" contype="0" pos="0 0.1 0" rgba="1.0 0.0 0.0 0.5" size="1 0.005 0.005" type="box"></geom>
		<geom conaffinity="0" contype="0" pos="0 0.1 0" rgba="0.0 1.0 0.0 0.5" size="0.005 1 0.005" type="box"></geom>
		<geom conaffinity="0" contype="0" pos="0 0.1 0" rgba="0.0 0.0 1.0 0.5" size="0.005 0.005 1" type="box"></geom>
	</body>

    
    <body name="robot0:base_link" pos="0 0 0.0" >
    <!-- <body name="robot0:base_link" pos="0 0 0.061875" > -->
        <!-- <geom mesh="base_link" name="robot0:base_link" material="robot0:base_mat" class="robot0:grey"></geom> -->
        <geom name="base_link_visual" type="mesh" contype="0" conaffinity="0" group="1" mesh="base_link" />
        <geom name="base_link_collision" size="0.4937 0.28545 0.061875" type="box" />
        <!-- <geom size="0.4937 0.28545 0.061875" pos="0 0 0.061875" type="box" /> -->

		<joint armature="0.0001" axis="1 0 0" damping="1e+11" name="robot0:slide0" pos="0 0 0" type="slide"></joint>
		<joint armature="0.0001" axis="0 1 0" damping="1e+11" name="robot0:slide1" pos="0 0 0" type="slide"></joint>
		<joint armature="0.0001" axis="0 0 1" damping="1e+0" name="robot0:slide2" pos="0 0 0" type="slide"></joint>
		<inertial diaginertia="1.2869 1.2236 0.9868" mass="70.1294" pos="-0.0036 0 0.0014" quat="0.7605 -0.0133 -0.0061 0.6491"></inertial>

		<!-- control the husky base -->
		<joint name="base_x_slide_joint" type="slide" pos="0 0 0" axis="1 0 0" limited="true" range="-1 0.5" damping="15000"/>
        <joint name="base_z_hinge_joint" type="hinge" pos="0 0 0" axis="0 0 1" limited="true" range="-0.57 0.57" damping="10000"/>
        <!-- <joint name="base_x_slide_joint" type="slide" pos="0 0 0" axis="1 0 0" limited="true" range="-1 0.5" damping="100"/> -->

        <!-- husky mobile robot body -->
        <geom size="0.39496 0.28545 0.061875" pos="0 0 0.185625" type="box" />
        <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="top_chassis" class="husky:orange" />

        <geom pos="0.48 0 0.091" type="mesh" contype="0" conaffinity="0" group="1" mesh="bumper" />
        <geom pos="-0.48 0 0.091" quat="1.32679e-06 0 0 1" type="mesh" contype="0" conaffinity="0" group="1" mesh="bumper" />

        <!-- This part will collide with arm, leading to a range (-2., 0.75), not (-3.14, 3.14) 2020.01.17-->
        <geom pos="0 0 0.225" type="mesh" contype="0" conaffinity="0" group="1" mesh="dual_arm_bulkhead" />
        <!-- <geom pos="0 0 0.225" type="mesh" mesh="dual_arm_bulkhead" /> -->

        <geom pos="0.3155 0.010414 0.44576" quat="0.000796327 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.3 0.3 0.3 1" mesh="flir-ptu-simple" />
        <geom size="0.0225 0.0325 0.022" pos="0.345 0.010414 0.44576" type="box" rgba="0.3 0.3 0.3 1" />
        <geom size="0.023 0.022 0.022" pos="0.3 0.020914 0.44576" type="box" rgba="0.3 0.3 0.3 1" />
        <geom size="0.005 0.0075" pos="0.271 0.020414 0.44576" quat="0.707388 0 0.706825 0" type="cylinder" rgba="0.3 0.3 0.3 1" />
        <geom pos="0.465 0 0.14" quat="1.32679e-06 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="sick-lms1xx" class="sick_lidar" />
        <geom size="0.0525 0.05 0.05" pos="0.465 1.32679e-07 0.19" quat="1.32679e-06 1 0 0" type="box" />
        <geom pos="0.125 -0.15 0.36" quat="0.653595 0.27036 0.270576 -0.653075" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="base" />
        <geom pos="0.125 -0.15 0.36" quat="0.653595 0.27036 0.270576 -0.653075" type="mesh" rgba="0.7 0.7 0.7 1" mesh="base" />
        <geom pos="0.125 0.15 0.36" quat="0.653595 -0.27036 -0.270576 -0.653075" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="base" />
        <geom pos="0.125 0.15 0.36" quat="0.653595 -0.27036 -0.270576 -0.653075" type="mesh" rgba="0.7 0.7 0.7 1" mesh="base" />
        <!-- husky wheels -->
        <body name="front_left_wheel_link" pos="0.256 0.2854 0.03282">
            <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="200.637" diaginertia="0.04411 0.02467 0.02467" />
            <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="wheel" class="wheel"/>
            <geom size="0.17775 0.05715" quat="0.707107 0.707106 0 0" type="cylinder" />
        </body>
        <body name="front_right_wheel_link" pos="0.256 -0.2854 0.03282">
            <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="200.637" diaginertia="0.04411 0.02467 0.02467" />
            <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="wheel" class="wheel"/>
            <geom size="0.17775 0.05715" quat="0.707107 0.707106 0 0" type="cylinder" />
        </body>
        <body name="rear_left_wheel_link" pos="-0.256 0.2854 0.03282">
            <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="200.637" diaginertia="0.04411 0.02467 0.02467" />
            <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="wheel" class="wheel"/>
            <geom size="0.17775 0.05715" quat="0.707107 0.707106 0 0" type="cylinder" />
        </body>
        <body name="rear_right_wheel_link" pos="-0.256 -0.2854 0.03282">
            <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="200.637" diaginertia="0.04411 0.02467 0.02467" />
            <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="wheel" class="wheel"/>
            <geom size="0.17775 0.05715" quat="0.707107 0.707106 0 0" type="cylinder" />
        </body>

        <!-- husky_ptu_tilt for camera -->
        <body name="husky_ptu_tilt_link" pos="0.345 -0.000586 0.51176" quat="0.707105 -0.707108 0 0">
            <inertial pos="0 0 0" mass="0.65" diaginertia="0.0001 0.0001 0.0001" />
            <joint name="husky_ptu_pan" pos="0 0 0" axis="0 -1 0" limited="true" range="-2.775 2.775" />
            <geom pos="-0.0295 0.011 0" quat="0.000796327 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.3 0.3 0.3 1" mesh="flir-ptu-simple" />
            <geom size="0.0225 0.0325 0.022" pos="0 0.011 0" type="box" rgba="0.3 0.3 0.3 1" />
            <geom size="0.023 0.022 0.022" pos="-0.045 0.0215 0" type="box" rgba="0.3 0.3 0.3 1" />
            <geom size="0.005 0.0075" pos="-0.074 0.021 0" quat="0.707388 0 0.706825 0" type="cylinder" rgba="0.3 0.3 0.3 1" />
            <body name="husky_ptu_tilted_link" pos="0 0 0" quat="0.707105 -0.707108 0 0">
                <inertial pos="0 1.63383e-07 -0.0613599" quat="0.499998 0.500002 0.499998 0.500002" mass="0.342012" diaginertia="0.000766531 0.000739434 0.000100971" />
                <joint name="husky_ptu_tilt" pos="0 0 0" axis="0 1 0" limited="true" range="-0.82 0.52" />
                <geom pos="0 1.63384e-07 -0.06136" quat="0.499998 0.50005 0.499998 0.499954" type="mesh" contype="0" conaffinity="0" group="1" mesh="pointgrey_bumblebee2" class="bb2_camera" />
                <geom size="0.0237 0.0785 0.018" pos="0 1.63384e-07 -0.06136" quat="-3.67321e-06 1 0 0" type="box" />
            </body>
        </body>

        <!-- left ur5 -->
        <include file="robot_ur5_l.xml"></include>

        <!-- right ur5 -->
        <include file="robot_ur5_r.xml"></include>

    </body>
</mujoco>
