<?xml version="1.0" encoding="utf-8"?>
<mujoco model="sawyer">
  <!--<geom name="floor" type="plane" pos="0 0 -0.3" size="0.6 1.2 0.5" rgba="0.13 0.20 0.24 1" contype="0" conaffinity="0"/>-->
  <!--<geom name="floor" type="plane" pos="0 0 -0.3" size="0.6 1.2 0.5" rgba="0.11 0.17 0.20 1" contype="0" conaffinity="0"/>-->
  <geom name="floor" type="plane" pos="0 0 -0.3" size="0.6 1.3 0.5" rgba="0.15 0.15 0.15 1" contype="0"
        conaffinity="0"/>
  <geom name="tableTop" type="box" pos="0 0.65 -0.45" size="0.5 0.5 0.45" rgba="0.70 0.72 0.78 1"
        contype="1" conaffinity="1" condim="4" density="6500"/>

  <!--rgba="0.75 0.77 0.79 1"-->
  <body name="base" pos="0 0 0" quat="1 0 0 1">
    <site name="basesite" pos="0 0 0" size="0.01"/>
    <inertial pos="0 0 0" mass="0" diaginertia="0 0 0"/>
    <body name="torso" pos="0 0 0">
      <inertial pos="0 0 0" mass="0.0001" diaginertia="1e-08 1e-08 1e-08"/>
      <geom 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">
      <!--<camera name="maincam" mode="fixed" fovy="32" euler="0.7 0 0" pos="0 -1.1 1.3"/>-->
      <!--<camera name="leftcam" mode="fixed" fovy="32" euler="0.7 0 -1.57" pos="-1.1 0 1.3"/>-->
      <!--<camera name="topcam" mode="fixed" fovy="32" euler="0.7 0 -1.57" pos="0 0 1.3"/>-->
      <!--this is the camera looking down from above the desk-->
      <camera name="topcam" pos="0.5 0 0.8" zaxis="0 0 1"/>
      <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 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 size="0.18 0.31" pos="-0.02 0 -0.29" type="cylinder" rgba="0.2 0.2 0.2 0"/>
    </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 type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="base"/>
      <geom size="0.08 0.12" pos="0 0 0.12" type="cylinder" rgba="0.5 0.1 0.1 0"/>
      <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.05 3.05" damping="0"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="l0"/>
        <geom size="0.07" pos="0.08 0 0.23" rgba="0.5 0.1 0.1 0"/>
        <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" damping="10"/> -->
          <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="head"/>
          <geom size="0.18" pos="0 0 0.08" rgba="0.5 0.1 0.1 0"/>
          <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 size="0.12 0.07 0.001" type="box" contype="0" conaffinity="0" group="1"
                  rgba="0.2 0.2 0.2 0"/>
            <geom size="0.001" rgba="0.2 0.2 0.2 0"/>
          </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"/>
            <site name="headsite" pos="0 0 0" size="0.01"/>
          </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" damping="10"/>-->
          <joint name="right_j1" pos="0 0 0" axis="0 0 1" limited="true" range="-3.8 -0.5" damping="0"/>
          <!--<joint name="right_j1" pos="0 0 0" axis="0 0 1" limited="true" range="0.8095 2.2736" damping="10"/>-->
          <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="l1"/>
          <geom size="0.07" pos="0 0 0.1225" rgba="0.5 0.1 0.1 0"/>
          <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.05 -0.15" damping="0"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="l2"/>
            <geom size="0.06 0.17" pos="0 0 0.08" type="cylinder" rgba="0.5 0.1 0.1 0"/>
            <body name="right_l3" pos="0 -0.042 0.26" quat="0.707107 -0.707107 0 0">
              <site name="armsite" pos="0 0 0" size="0.01"/>
              <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"
                     damping="0"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="l3"/>
              <geom size="0.06" pos="0 -0.01 -0.12" rgba="0.5 0.1 0.1 0"/>
              <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 -1.6"
                       damping="0"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat" mesh="l4"/>
                <geom size="0.045 0.15" pos="0 0 0.11" type="cylinder" rgba="0.5 0.1 0.1 0"/>
                <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" damping="0"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat"
                        mesh="l5"/>
                  <geom size="0.06" pos="0 0 0.1" rgba="0.5 0.1 0.1 0"/>
                  <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" damping="0"/>
                    <geom type="mesh" contype="0" conaffinity="0" group="1" material="arm_mat"
                          mesh="l6"/>
                    <geom size="0.055 0.025" pos="0 0.015 -0.01" type="cylinder"
                          rgba="0.5 0.1 0.1 0"/>
                    <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 type="mesh" contype="1" conaffinity="1" group="1" material="arm_mat"
                            pos="0 0 0.03" mesh="eGripperBase"/>

                      <geom size="0.035 0.014" pos="0 0 0.015" type="cylinder" rgba="0 0 0 1"/>

                      <!--  ================= BEGIN GRIPPER ================= /-->
                      <body name="hand" pos="0 0 0.12" quat="-1 0 1 0">
                        <body name="frame" pos="0 0 0" quat="0, 0, -0.4871745, -0.8733046">
                          <camera name="tightcam" mode="fixed" fovy="12" euler="1.15 0 .7" pos="0.95 -1.1 0.6"/>
                        </body>
                        <site name="endEffector" pos="0.03 0 0" size="0.01" rgba="1 0 0 0.01"/>
                        <!--  <geom name="rail" type="box" pos="-0.05 0 0" density="7850" size="0.005 0.055 0.005" friction="0.001 0.005 0.0001" rgba="0.5 0.5 0.5 1.0" /> -->
                        <geom name="rail" type="box" pos="-0.05 0 0" density="7850" size="0.005 0.055 0.005"
                              rgba="0.5 0.5 0.5 1.0"/>
                        <body name="rightclaw" pos="0 -0.04 0">
                          <geom name="rightclaw_it" margin="0.001" type="box" user="0" pos="0 0 0" size="0.045 0.005 0.015" rgba="0.5 0.5 0.5 1.0" condim="4" friction="1 0.05 0.01"/>
                          <joint name="r_close" pos="0 0 0" axis="0 1 0" range="0 0.03" armature="100" damping="0" limited="true" type="slide"/>
                          <site name="rightEndEffector" pos="0.035 0 0" size="0.01"/>
                        </body>
                        <body name="leftclaw" pos="0 0.04 0">
                          <geom name="leftclaw_it" margin="0.001" type="box" user="0" pos="0 0 0" size="0.045 0.005 0.015" rgba="0 0.75 0.75 1.0" condim="4" friction="1 0.05 0.01"/>
                          <joint name="l_close" pos="0 0 0" axis="0 1 0" range="-0.03 0" armature="100" damping="0" limited="true" type="slide"/>
                          <site name="leftEndEffector" pos="0.035 0 0" size="0.01"/>
                        </body>
                      </body>
                      <!--  ================= END GRIPPER ================= /-->
                    </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 size="0.06" pos="0 0.01 0.26"
                        rgba="0.2 0.2 0.2 0"
                        contype="0"
                        conaffinity="0"
                  />
                </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 size="0.06" pos="0 0 0.26" rgba="0.2 0.2 0.2 0"
                    contype="0"
                    conaffinity="0"
              />
            </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 size="0.07 0.07" pos="0 0 0.035" type="cylinder" rgba="0.2 0.2 0.2 0"/>
          </body>
        </body>
      </body>
    </body>
  </body>

  <body mocap="true" name="mocap" pos="0 0 0">
    <!--For debugging, set the alpha to 1-->
    <site pos="0 0 0" name="mocap" rgba="1 1 1 0.1" size="0.01" type="sphere"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.7" size="0.005 0.005 0.005" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="1 0.005 0.005" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="0.005 1 0.001" type="box"/>
    <geom conaffinity="0" contype="0" pos="0 0 0" rgba="0 0.5 0 0.1" size="0.005 0.005 1" type="box"/>
  </body>

</mujoco>
