<mujoco model="hdt">
	<compiler angle="radian" coordinate="local"></compiler>

	<option timestep="2e-3"
			iterations="10"
			tolerance="1e-10"
			impratio="10"
			gravity="0 0 -9.81"
			solver="PGS"
			cone="elliptic" />
	<size nuserdata="100" njmax="500" nconmax="1000" nstack="600000" nuser_body="9" nuser_jnt="1" nuser_site="1" nuser_tendon="1" nuser_actuator="12" nuser_sensor="1" />

	<!-- <visual>
		<map fogstart="3" fogend="5" force="0.1" znear="0.1"/>
		<quality shadowsize="2048" offsamples="8"/>
		<global offwidth="800" offheight="800"/>
    </visual> 
         -->
    <actuator>
      <velocity ctrllimited="true" ctrlrange="-2.0000 2.0000" joint="joint1" kv="1"/>
      <velocity ctrllimited="true" ctrlrange="-1.5708 1.5708" joint="joint2" kv="1"/>
      <velocity ctrllimited="true" ctrlrange="-3.0708 1.2708" joint="joint3" kv="1"/>
      <velocity ctrllimited="true" ctrlrange="-2.5708 2.5708" joint="joint4" kv="1"/>
      <velocity ctrllimited="true" ctrlrange="-1.5708 1.5708" joint="joint5" kv="1"/>
	  <velocity ctrllimited="true" ctrlrange="-1.5708 2.5708" joint="joint6" kv="1"/>
    </actuator>

    <asset>
        <mesh name="H026994 - A35 M-2 PEDESTAL ASSY - SHAREABLE" file="meshes/H026994 - A35 M-2 PEDESTAL ASSY - SHAREABLE.STL" />
        <mesh name="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" file="meshes/H024840 - A35 M-2 DRIVE MODULE, SHAREABLE.STL" />
        <mesh name="H024847 - A35 M-2 PROXIMAL TUBE ASSY - SHAREABLE" file="meshes/H024847 - A35 M-2 PROXIMAL TUBE ASSY - SHAREABLE.STL" />
        <mesh name="H024848 - A35 M-2 DISTAL TUBE ASSY - SHAREABLE" file="meshes/H024848 - A35 M-2 DISTAL TUBE ASSY - SHAREABLE.STL" />
        <mesh name="H024842 - A35 M-2 PINCER ASSY - SHAREABLE, UNDERWATER" file="meshes/H024842 - A35 M-2 PINCER ASSY - SHAREABLE, UNDERWATER.STL" />
        <mesh name="H022927 - A35 PINCER FINGER, LONG, OVERMOLD ASSY - SHAREABLE" file="meshes/H022927 - A35 PINCER FINGER, LONG, OVERMOLD ASSY - SHAREABLE.STL" />
    </asset>

    <default>
        <joint damping=".1"/>
    </default>

    <contact>
        <!-- <exclude body1="robot0:base_link" body2="bluerov2_base"></exclude> -->
        <exclude body1="base" body2="link1"></exclude>
        <exclude body1="link1" body2="link2"></exclude>
        <exclude body1="link2" body2="link3"></exclude>
        <exclude body1="link3" body2="link4"></exclude>
        <exclude body1="link4" body2="link5"></exclude>
        <exclude body1="link5" body2="link6"></exclude>
        <exclude body1="link6" body2="right_hand"></exclude>
        <!-- <exclude body1="right_hand" body2="hdt_gripper"></exclude> -->
    </contact>

    <worldbody>
        <body name="base" pos="0. 0. 0.25">
    
            <geom type="mesh"  rgba="0.278 0.278 0.278 1.0"  mesh="H026994 - A35 M-2 PEDESTAL ASSY - SHAREABLE" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0"  mesh="H026994 - A35 M-2 PEDESTAL ASSY - SHAREABLE" />
            <body name="link1" pos="0 0 0.08335" quat="0.707107 0 0 -0.707107">
                <inertial pos="0 0 0" mass="1" diaginertia="0.005 0.005 0.005" />
                <joint name="joint1" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping=".1" />
                <geom name="drive2_link_collision" type="mesh" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0"  mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                <body name="link2" pos="0.07155 0 0" quat="0.707107 0 -0.707107 0">
                    <inertial pos="0.17855 0 0" quat="0.5 0.5 0.5 0.5" mass="3" diaginertia="0.0787602 0.0787602 0.015" />
                    <joint name="joint2" pos="0 0 0" axis="0 0 1" limited="true" range="-1.5708 1.5708" damping=".1" />
                    <geom quat="0 1 0 0" type="mesh" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                    <geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                    <geom pos="0.17855 0 0" type="mesh" mesh="H024847 - A35 M-2 PROXIMAL TUBE ASSY - SHAREABLE" />
                    <geom pos="0.17855 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024847 - A35 M-2 PROXIMAL TUBE ASSY - SHAREABLE" />
                    <geom pos="0.3571 0 0" quat="0 0 1 0" type="mesh" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                    <geom pos="0.3571 0 0" quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                    <body name="link3" pos="0.3571 0 0.07155">
                        <inertial pos="0 0 0" quat="0.5 0.5 0.5 -0.5" mass="1" diaginertia="0.005 0.005 0.005" />
                        <joint name="joint3" pos="0 0 0" axis="0 0 1" limited="true" range="-4.49 1.35" damping=".1" />
                        <geom  name="drive3_link_collision" quat="0.5 0.5 0.5 -0.5" type="mesh" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                        <geom quat="0.5 0.5 0.5 -0.5" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                        <body name="link4" pos="0 0.142 0" quat="0.707107 -0.707107 0 0">
                            <inertial pos="0 0 0.064775" mass="2" diaginertia="0.0183916 0.0183916 0.01" />
                            <joint name="joint4" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping=".1" />
                            <geom  name="drive41_link_collision"  quat="0 1 0 0" type="mesh" mesh="H024848 - A35 M-2 DISTAL TUBE ASSY - SHAREABLE" />
                            <geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024848 - A35 M-2 DISTAL TUBE ASSY - SHAREABLE" />
                            <geom  name="drive42_link_collision"  pos="0 0 0.12955" quat="-0.5 0.5 -0.5 -0.5" type="mesh" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                            <geom pos="0 0 0.12955" quat="-0.5 0.5 -0.5 -0.5" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                            <body name="link5" pos="0 -0.07155 0.12955" quat="-0.5 -0.5 0.5 -0.5">
                                <inertial pos="0 0 0" mass="1" diaginertia="0.005 0.005 0.005" />
                                <joint name="joint5" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping=".1" />
                                <geom  name="drive5_link_collision"  quat="0 -0.707107 0 0.707107" type="mesh" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                                <geom quat="0 -0.707107 0 0.707107" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024840 - A35 M-2 DRIVE MODULE, SHAREABLE" />
                                <body name="link6" pos="0.0885 0 0" quat="0.707107 0 0.707107 0">
                                    <inertial pos="0 0 0" mass="1" diaginertia="0.005 0.005 0.005" />
                                    <joint name="joint6" pos="0 0 0" axis="0 0 1" limited="true" range="-6.28319 6.28319" damping=".1" />
                                    <geom name="drive6_link_collision" quat="0.5 0.5 0.5 -0.5" type="mesh" mesh="H024842 - A35 M-2 PINCER ASSY - SHAREABLE, UNDERWATER" />
                                    <geom quat="0.5 0.5 0.5 -0.5" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.278 0.278 0.278 1.0" mesh="H024842 - A35 M-2 PINCER ASSY - SHAREABLE, UNDERWATER" />
                                    <body name="right_hand" pos="0 0 0">
                                        <!-- right gripper gets added here -->        
                                    </body>                            
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </worldbody>
</mujoco>