<mujoco model='ur3e_fish'>
  <compiler angle='radian' meshdir="./mesh/ur3e/collision/" texturedir="./mesh/ur3e/visual/" />
  <option timestep="0.005" collision='predefined' />

  <asset>
      <mesh name="base"     file="base.stl" />
      <mesh name="shoulder" file="shoulder.stl" />
      <mesh name="upperarm" file="upperarm.stl" />
      <mesh name="forearm"  file="forearm.stl" />
      <mesh name="wrist1"   file="wrist1.stl" />
      <mesh name="wrist2"   file="wrist2.stl" />
      <mesh name="wrist3"   file="wrist3.stl" />
      <mesh name="fish"     file="../../fish/fish.stl" />
      <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512"/>
      <material name="MatPlane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
  </asset>

  <worldbody>
    <!-- Lights -->
    <light name="light0" diffuse="1 1 1" pos="0 0 3" dir="0 0 -1" castshadow="false"/>
    <light name="light1" diffuse="1 1 1" pos="1 -1 -.1" dir="-1 1 1" castshadow="false"/>
    <light name="light2" diffuse="1 1 1" pos="1.5 0 3"  dir="0 0 -1" castshadow="false"/>
    <!-- Environment -->
    <geom name="floor" pos="0 0 -.913" size="5 5 .125" type="plane" material="MatPlane" condim="3"/>
    <body name="table" pos="0.4 0 0" >
      <geom name="table" pos="0.0 0 0" type="plane" size=".5 .6 0.1" group = "1" rgba=".1 .1 .1 1" />
    </body>

    <body name="fish" pos="0.4 0.1 0.02" quat="1 0 0 0" >
      <joint name="fish" type="free" limited="false"/>
        <geom name="fish" pos="0 0 0" quat="1 0 0 0" type="mesh" group="1" mesh="fish" />
        <site name="fish_front" pos="-0.038 0 0.01"  quat="1 0 0 0" type="sphere" size="1e-3" group="2" rgba="1 1 0 1" />
        <site name="fish_rear"  pos="0.031 0 0.01"  quat="0 0 0 1" type="sphere" size="1e-3" group="2" rgba="1 1 0 1" />
    </body>

    <!-- Robot -->
    <body name="base" pos="0 0 0">
        <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="base" />
        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="base" />
        <body name="shoulder_link" pos="0 0 0.152">
            <inertial pos="0 0 0" mass="2" diaginertia="0.00809316 0.00809316 0.005625" />
            <joint name="shoulder_pan_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="shoulder" />
            <geom name="shoulder" type="mesh" rgba="0.7 0.7 0.7 1" mesh="shoulder" />
            <body name="upper_arm_link" pos="0 0.12 0" quat="0.707107 0 0.707107 0">
                <inertial pos="0 0 0.122" mass="3.42" diaginertia="0.0217771 0.0217771 0.00961875" />
                <joint name="shoulder_lift_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="upperarm" />
                <geom name="upperarm" type="mesh" rgba="0.7 0.7 0.7 1" mesh="upperarm" />
                <body name="forearm_link" pos="0 -0.093 0.244">
                    <inertial pos="0 0 0.1065" mass="1.26" diaginertia="0.00653562 0.00653562 0.00354375" />
                    <joint name="elbow_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="forearm" />
                    <geom name="forearm" type="mesh" rgba="0.7 0.7 0.7 1" mesh="forearm" />
                    <body name="wrist_1_link" pos="0 0 0.213" quat="0.707107 0 0.707107 0">
                        <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="0.8" diaginertia="0.00225 0.002085 0.002085" />
                        <joint name="wrist_1_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                        <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist1" />
                        <geom name="wrist1" type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist1" />
                        <body name="wrist_2_link" pos="0 0.104 0">
                            <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="0.8" diaginertia="0.00225 0.002085 0.002085" />
                            <joint name="wrist_2_joint" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" />
                            <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist2" />
                            <geom name="wrist2" type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist2" />
                            <body name="wrist_3_link" pos="0 0 0.085">
                                <inertial pos="0 0.000233618 0" quat="0.5 0.5 -0.5 0.5" mass="0.351" diaginertia="0.000991097 0.000918909 0.000912204" />
                                <joint name="wrist_3_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                                <geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" mesh="wrist3" />
                                <geom name="wrist3" type="mesh" rgba="0.7 0.7 0.7 1" mesh="wrist3" />
                                <geom size="0.005 0.005 0.005" pos="0 0.082 0" quat="0.707107 0 0 0.707107" type="box" group="2" />
                                <site name="end_effector" type="sphere" size="1e-3" pos="0 0.092 0" group="2" rgba="0 1 0 1" />
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
      </body>

      <!-- Ghost representing desired pose -->
      <body name="ghost" pos="0.6 0.15 0.015" >
          <geom name="ghost" pos="0 0 0" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" mesh="fish" rgba="0.1 0.8 0.1 1" />
      </body>
  </worldbody>

  <!-- Actuators -->
  <actuator>
    <motor name="shoulder_pan"  joint="shoulder_pan_joint"  ctrllimited="true" ctrlrange="-330 330" />
    <motor name="shoulder_lift" joint="shoulder_lift_joint" ctrllimited="true" ctrlrange="-330 330" />
    <motor name="elbow"         joint="elbow_joint"         ctrllimited="true" ctrlrange="-150 150" />
    <motor name="wrist_1"       joint="wrist_1_joint"       ctrllimited="true" ctrlrange="-54 54" />
    <motor name="wrist_2"       joint="wrist_2_joint"       ctrllimited="true" ctrlrange="-54 54" />
    <motor name="wrist_3"       joint="wrist_3_joint"       ctrllimited="true" ctrlrange="-54 54" />
  </actuator>

  <!-- Contacts -->
  <contact>
    <pair geom1="wrist1"    geom2="fish" />
    <pair geom1="wrist2"    geom2="fish" />
    <pair geom1="wrist3"    geom2="fish" />
    <pair geom1="upperarm"  geom2="table"  />
    <pair geom1="forearm"   geom2="table"  />
    <pair geom1="wrist1"    geom2="table"  />
    <pair geom1="wrist2"    geom2="table" />
    <pair geom1="wrist3"    geom2="table"  />
    <pair geom1="table"     geom2="fish" />
  </contact>

  <!-- Key frames -->
  <keyframe>
      <key name="init_pose" qpos="0 0.25 0.02 1 0 0 0 0.5 -0.75 1.75 -1 -1 0" />
    <!--<key name="init_pose" qpos="0.4 0.15 0.02 1 0 0 0 0.35 -0.95 2.25 -1.25 1.5 0" />-->
  </keyframe>


</mujoco>
