<mujoco model='sawyer_fish'>
  <compiler meshdir="./mesh/sawyer/" angle='radian'/>
  <option timestep="0.005" collision='predefined'/>

  <asset>
      <mesh name="pedestal"   file="pedestal.stl" />
      <mesh name="base"       file="base.stl" />
      <mesh name="l0"         file="l0.stl" />
      <mesh name="head"       file="head.stl" />
      <mesh name="l1"         file="l1.stl" />
      <mesh name="l2"         file="l2.stl" />
      <mesh name="l3"         file="l3.stl" />
      <mesh name="l4"         file="l4.stl" />
      <mesh name="l5"         file="l5.stl" />
      <mesh name="l6"         file="l6.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"/>
    <geom name="table" pos="1.1 0 0.025" type="plane" size=".4 .6 0.1" group = "1" rgba=".1 .1 .1 1"/>

    <body name="fish" pos="1.1 0 0.055" 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" />
    </body>

    <!-- Robot -->
     <body name="base" pos=".25 0 0">
         <inertial pos="0 0 0" mass="0" diaginertia="0 0 0" />
         <body name="controller_box" pos="0 0 0">
             <inertial pos="-0.325 0 -0.38" mass="46.64" diaginertia="1.71363 1.27988 0.809981" />
             <geom name="controller_box" size="0.11 0.2 0.265" pos="-0.325 0 -0.38" type="box" />
         </body>
         <body name="pedestal_feet" pos="0 0 0">
             <inertial pos="-0.1225 0 -0.758" mass="167.09" diaginertia="8.16095 9.59375 15.0785" />
             <geom name="pedestal_feet" size="0.385 0.35 0.155" pos="-0.1225 0 -0.758" type="box" />
         </body>
         <body name="torso" pos="0 0 0">
             <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08" />
             <geom name="torso" size="0.05 0.05 0.05" type="box" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" />
         </body>
         <body name="pedestal" pos="0 0 0">
             <inertial pos="0 0 0" quat="0.659267 -0.259505 -0.260945 0.655692" mass="60.864" diaginertia="6.0869 5.81635 4.20915" />
             <geom name="pedestal" pos="0.26 0.345 -0.91488" quat="0.5 0.5 -0.5 -0.5" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" mesh="pedestal" />
             <geom name="pedestal_coll" size="0.18 0.31" pos="-0.02 0 -0.29" type="cylinder" rgba="0.2 0.2 0.2 1" />
         </body>
         <body name="right_arm_base_link" pos="0 0 0">
             <inertial pos="-0.0006241 -2.8025e-05 0.065404" quat="-0.209285 0.674441 0.227335 0.670558" mass="2.0687" diaginertia="0.00740351 0.00681776 0.00672942" />
             <geom name="right_arm_base_link" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="base" />
              <geom name="right_base_coll" size="0.08 0.12" pos="0 0 0.12" type="cylinder" rgba="0.5 0.1 0.1 1" />
             <body name="right_l0" pos="0 0 0.08">
                 <inertial pos="0.024366 0.010969 0.14363" quat="0.894823 0.00899958 -0.170275 0.412573" mass="5.3213" diaginertia="0.0651588 0.0510944 0.0186218" />
                 <joint name="right_j0" pos="0 0 0" axis="0 0 1" limited="true" range="-3.0503 3.0503" />
                 <geom name="right_l0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l0" />
                 <geom name="right_l0_coll" size="0.07" pos="0.08 0 0.23" rgba="0.5 0.1 0.1 1" />
                 <body name="head" pos="0 0 0.2965">
                     <inertial pos="0.0053207 -2.6549e-05 0.1021" quat="0.999993 7.08405e-05 -0.00359857 -0.000626247" mass="1.5795" diaginertia="0.0118334 0.00827089 0.00496574" />
                     <!-- <joint name="head_pan" pos="0 0 0" axis="0 0 1" limited="true" range="-5.0952 0.9064" /> -->
                     <geom name="head" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="head" />
                     <!-- <geom size="0.18" pos="0 0 0.08" rgba="0.5 0.1 0.1 1" /> -->
                     <body name="screen" pos="0.03 0 0.105" quat="0.5 0.5 0.5 0.5">
                         <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08" />
                         <geom name="screen" size="0.12 0.07 0.001" type="box" contype="0" conaffinity="0" group="1" rgba="0.2 0.2 0.2 1" />
                          <geom name="screen_coll" size="0.001" rgba="0.2 0.2 0.2 1" />
                     </body>
                     <body name="head_camera" pos="0.0228027 0 0.216572" quat="0.342813 -0.618449 0.618449 -0.342813">
                         <inertial pos="0.0228027 0 0.216572" quat="0.342813 -0.618449 0.618449 -0.342813" mass="0" diaginertia="0 0 0" />
                     </body>
                 </body>
                 <body name="right_torso_itb" pos="-0.055 0 0.22" quat="0.707107 0 -0.707107 0">
                     <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08" />
                 </body>
                 <body name="right_l1" pos="0.081 0.05 0.237" quat="0.5 -0.5 0.5 0.5">
                     <inertial pos="-0.0030849 -0.026811 0.092521" quat="0.424888 0.891987 0.132364 -0.0794296" mass="4.505" diaginertia="0.0224339 0.0221624 0.0097097" />
                     <joint name="right_j1" pos="0 0 0" axis="0 0 1" limited="true" range="-3.8095 2.2736" />
                     <geom name="right_l1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l1" />
                      <geom name="right_l1_coll" size="0.07" pos="0 0 0.1225" rgba="0.5 0.1 0.1 1" />
                     <body name="right_l2" pos="0 -0.14 0.1425" quat="0.707107 0.707107 0 0">
                         <inertial pos="-0.00016044 -0.014967 0.13582" quat="0.707831 -0.0524761 0.0516007 0.702537" mass="1.745" diaginertia="0.0257928 0.025506 0.00292515" />
                         <joint name="right_j2" pos="0 0 0" axis="0 0 1" limited="true" range="-3.0426 3.0426" />
                         <geom name="right_l2" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l2" />
                          <geom name="right_l2_coll" size="0.06 0.17" pos="0 0 0.08" type="cylinder" rgba="0.5 0.1 0.1 1" />
                         <body name="right_l3" pos="0 -0.042 0.26" quat="0.707107 -0.707107 0 0">
                             <inertial pos="-0.0048135 -0.0281 -0.084154" quat="0.902999 0.385391 -0.0880901 0.168247" mass="2.5097" diaginertia="0.0102404 0.0096997 0.00369622" />
                             <joint name="right_j3" pos="0 0 0" axis="0 0 1" limited="true" range="-3.0439 3.0439" />
                             <geom name="right_l3" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l3" />
                              <geom name="right_l3_coll" size="0.06" pos="0 -0.01 -0.12" rgba="0.5 0.1 0.1 1" />
                             <body name="right_l4" pos="0 -0.125 -0.1265" quat="0.707107 0.707107 0 0">
                                 <inertial pos="-0.0018844 0.0069001 0.1341" quat="0.803612 0.031257 -0.0298334 0.593582" mass="1.1136" diaginertia="0.0136549 0.0135493 0.00127353" />
                                 <joint name="right_j4" pos="0 0 0" axis="0 0 1" limited="true" range="-2.9761 2.9761" />
                                 <geom name="right_l4" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l4" />
                                  <geom name="right_l4_coll" size="0.045 0.15" pos="0 0 0.11" type="cylinder" rgba="0.5 0.1 0.1 1" />
                                 <body name="right_arm_itb" pos="-0.055 0 0.075" quat="0.707107 0 -0.707107 0">
                                     <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08" />
                                 </body>
                                 <body name="right_l5" pos="0 0.031 0.275" quat="0.707107 -0.707107 0 0">
                                     <inertial pos="0.0061133 -0.023697 0.076416" quat="0.404076 0.9135 0.0473125 0.00158335" mass="1.5625" diaginertia="0.00474131 0.00422857 0.00190672" />
                                     <joint name="right_j5" pos="0 0 0" axis="0 0 1" limited="true" range="-2.9761 2.9761" />
                                     <geom name="right_l5" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l5" />
                                     <geom name="right_l5_coll" size="0.06" pos="0 0 0.1" rgba="0.5 0.1 0.1 1" />
                                     <site name="link5" pos="0 0 0.15" type="sphere" size="1e-3" group="2" rgba="0 1 0 1" />
                                     <body name="right_hand_camera" pos="0.039552 -0.033 0.0695" quat="0.707107 0 0.707107 0">
                                         <inertial pos="0.039552 -0.033 0.0695" quat="0.707107 0 0.707107 0" mass="0" diaginertia="0 0 0" />
                                     </body>
                                     <body name="right_wrist" pos="0 0 0.10541" quat="0.707107 0.707107 0 0">
                                         <inertial pos="0 0 0.10541" quat="0.707107 0.707107 0 0" mass="0" diaginertia="0 0 0" />
                                     </body>
                                     <body name="right_l6" pos="0 -0.11 0.1053" quat="0.0616248 0.06163 -0.704416 0.704416">
                                         <inertial pos="-8.0726e-06 0.0085838 -0.0049566" quat="0.479044 0.515636 -0.513069 0.491322" mass="0.3292" diaginertia="0.000360258 0.000311068 0.000214974" />
                                         <joint name="right_j6" pos="0 0 0" axis="0 0 1" limited="true" range="-4.7124 4.7124" />
                                         <geom name="right_l6" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.5 0.1 0.1 1" mesh="l6" />
                                         <geom name="right_l6_coll" size="0.055 0.025" pos="0 0.015 -0.01" type="cylinder" rgba="0.5 0.1 0.1 1" />
                                         <body name="right_hand" pos="0 0 0.0245" quat="0.707107 0 0 0.707107">
                                             <inertial pos="1e-08 1e-08 1e-08" quat="0.820473 0.339851 -0.17592 0.424708" mass="1e-08" diaginertia="1e-08 1e-08 1e-08" />
                                              <geom name="right_hand_coll" size="0.035 0.015" pos="0 0 0.02" type="cylinder" />
                                              <site name="end_effector" type="sphere" size="1e-3" pos="0 0 6e-3" group="2" rgba="0 1 0 1" />
                                         </body>
                                     </body>
                                 </body>
                                 <body name="right_l4_2" pos="0 0 0">
                                     <inertial pos="1e-08 1e-08 1e-08" quat="0.820473 0.339851 -0.17592 0.424708" mass="1e-08" diaginertia="1e-08 1e-08 1e-08" />
                                     <geom name="right_l4_2" size="0.06" pos="0 0.01 0.26" />
                                 </body>
                             </body>
                         </body>
                         <body name="right_l2_2" pos="0 0 0">
                             <inertial pos="1e-08 1e-08 1e-08" quat="0.820473 0.339851 -0.17592 0.424708" mass="1e-08" diaginertia="1e-08 1e-08 1e-08" />
                             <geom name="right_l2_2" size="0.06" pos="0 0 0.26" />
                         </body>
                     </body>
                     <body name="right_l1_2" pos="0 0 0">
                         <inertial pos="1e-08 1e-08 1e-08" quat="0.820473 0.339851 -0.17592 0.424708" mass="1e-08" diaginertia="1e-08 1e-08 1e-08" />
                         <geom name="right_l1_2" size="0.07 0.07" pos="0 0 0.035" type="cylinder" />
                     </body>
                 </body>
             </body>
         </body>
     </body>

      <!-- Ghost representing desired pose -->
      <body name="ghost" pos="1.2 0 0.035" >
          <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="right_j0"    joint="right_j0" ctrllimited="true" ctrlrange="-80 80" />
    <motor name="right_j1"    joint="right_j1" ctrllimited="true" ctrlrange="-80 80" />
    <motor name="right_j2"    joint="right_j2" ctrllimited="true" ctrlrange="-40 40" />
    <motor name="right_j3"    joint="right_j3" ctrllimited="true" ctrlrange="-40 40" />
    <motor name="right_j4"    joint="right_j4" ctrllimited="true" ctrlrange="-9 9" />
    <motor name="right_j5"    joint="right_j5" ctrllimited="true" ctrlrange="-9 9" />
    <motor name="right_j6"    joint="right_j6" ctrllimited="true" ctrlrange="-9 9" />
  </actuator>

  <!-- Contacts -->
  <contact>
    <pair geom1="right_l6" geom2="fish" />
    <pair geom1="right_l5" geom2="fish" />
    <pair geom1="right_l4" geom2="fish" />
    <pair geom1="right_l6" geom2="table"  />
    <pair geom1="right_l5" geom2="table" />
    <pair geom1="right_l4" geom2="table"  />
    <pair geom1="table"    geom2="fish" />
  </contact>

  <!-- Key frames -->
  <keyframe>
    <key name="init_pose" qpos="1.1 0 0.045 1 0 0 0 -0.125 -0.433 0 1.6 -0.15 -0.923 0" />
  </keyframe>


</mujoco>
