<mujoco model="go1">
  <compiler angle="radian" autolimits="true"/>

  <option timestep="0.005" integrator="Euler" iterations="60">
    <flag eulerdamp="disable"/>
  </option>

  <custom>
    <numeric data="30" name="max_contact_points"/>
    <numeric data="12" name="max_geom_pairs"/>
  </custom>

  <default>
    <default class="go1">
      <geom condim="1"/>
      <joint axis="0 1 0" armature="0.005" damping="0.5"/>
      <!-- <position forcerange="-23.7 23.7" inheritrange="1" kp="35"/> -->
      <default class="abduction">
        <joint axis="1 0 0" range="-0.863 0.863" frictionloss="0.3"/>
      </default>
      <default class="hip">
        <joint range="-0.686 4.501" frictionloss="0.3"/>
      </default>
      <default class="knee">
        <joint range="-2.818 -0.888" frictionloss="1.0"/>
        <position forcerange="-35.55 35.55"/>
      </default>
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" material="dark"/>
      </default>
      <default class="collision">
        <geom group="3" type="capsule"/>
        <default class="hip_left1">
          <geom size="0.046 0.02" pos="0 0.045 0" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="hip_left2">
          <geom size="0.031 0.02" pos="0 0.065 0" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="hip_left3">
          <geom size="0.046 0.02" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="hip_right1">
          <geom size="0.046 0.02" pos="0 -0.045 0" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="hip_right2">
          <geom size="0.031 0.02" pos="0 -0.065 0" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="hip_right3">
          <geom size="0.046 0.02" quat="1 1 0 0" type="cylinder"/>
        </default>
        <default class="thigh1">
          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.213"/>
        </default>
        <default class="calf1">
          <geom type="capsule" size="0.015" fromto="0 0 0 0 0 -0.213"/>
        </default>
        <default class="foot">
          <geom type="sphere" size="0.023" pos="0 0 -0.213" solimp="0.9 .99 0.001" priority="10" condim="3"/>
        </default>
      </default>
    </default>
  </default>

  <asset>
    <material name="dark" rgba="0.2 0.2 0.2 1"/>
    <mesh class="go1" file="./assets/trunk.stl"/>
    <mesh class="go1" file="./assets/hip.stl"/>
    <mesh class="go1" file="./assets/thigh_mirror.stl"/>
    <mesh class="go1" file="./assets/calf.stl"/>
    <mesh class="go1" file="./assets/thigh.stl"/>
  </asset>

  <worldbody>
    <body name="trunk" pos="0 0 0.4" childclass="go1">
      <camera name="track" pos="0.846 -1.3 0.316" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940" mode="trackcom"/>
      <camera name="top" pos="-1 0 1" xyaxes="0 -1 0 0.7 0 0.7" mode="trackcom"/>
      <camera name="side" pos="0 -2 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
      <!-- <camera name="back" pos="-2.4 0 0.8" target="trunk" mode="targetbody" trackposspeed="2" trackrotspeed="10"/> -->
      <site name="head" pos="0.3 0 0" rgba="1 0 0 1" size="0.02" group="5"/>
      <inertial pos="0.0223 0.002 -0.0005" quat="-0.00342088 0.705204 0.000106698 0.708996" mass="5.204" diaginertia="0.0716565 0.0630105 0.0168101"/>
      <freejoint/>
      <geom class="visual" mesh="trunk"/>
      <geom name='trunk' class="collision" quat="1 0 0 0" pos="0.24 0 0" size="0.05 0.05 0.05" type="box"/>
      <!-- <geom name='trunk1' class="collision" quat="1 0 1 0" pos="0 -0.04 0" size="0.058 0.125" type="cylinder"/> -->
      <!-- <geom name='trunk2' class="collision" quat="1 0 1 0" pos="0 +0.04 0" size="0.058 0.125" type="cylinder"/> -->
      <site name="imu" pos="-0.01592 -0.06659 -0.00617" group="5"/>
      <body name="FR_hip" pos="0.1881 -0.04675 0">
        <inertial pos="-0.0049166 0.00762615 -8.865e-05" quat="0.507341 0.514169 0.495027 0.482891" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
        <joint class="abduction" name="FR_hip_joint"/>
        <geom class="visual" mesh="hip" quat="1 0 0 0"/>
        <geom name="fr_hip" class="hip_right1"/> 
        <body name="FR_thigh" pos="0 -0.08 0">
          <inertial pos="-0.00304722 0.019315 -0.0305004" quat="0.65243 -0.0272313 0.0775126 0.753383" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
          <joint class="hip" name="FR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom name="fr_thigh1" class="thigh1"/> 
          <!-- removed: <geom name="fr_thigh2" class="thigh2"/> (simplified to single capsule) --> 
          <!-- removed: <geom name="fr_thigh3" class="thigh3"/> (simplified to single capsule) --> 
          <body name="FR_calf" pos="0 0 -0.213">
            <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
            <joint class="knee" name="FR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom name="fr_calf1" class="calf1"/> 
            <!-- removed: <geom name="fr_calf2" class="calf2"/> (simplified to single capsule) --> 
            <geom name="FR_foot" class="foot"/>
            <site name="FR" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
          </body>
        </body>
      </body>
      <body name="FL_hip" pos="0.1881 0.04675 0">
        <inertial pos="-0.0049166 -0.00762615 -8.865e-05" quat="0.482891 0.495027 0.514169 0.507341" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
        <joint class="abduction" name="FL_hip_joint"/>
        <geom class="visual" mesh="hip"/>
        <geom name="fl_hip" class="hip_left1"/> 
        <body name="FL_thigh" pos="0 0.08 0">
          <inertial pos="-0.00304722 -0.019315 -0.0305004" quat="0.753383 0.0775126 -0.0272313 0.65243" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
          <joint class="hip" name="FL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom name="fl_thigh1" class="thigh1"/> 
          <!-- removed: <geom name="fl_thigh2" class="thigh2"/> (simplified to single capsule) --> 
          <!-- removed: <geom name="fl_thigh3" class="thigh3"/> (simplified to single capsule) --> 
          <body name="FL_calf" pos="0 0 -0.213">
            <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
            <joint class="knee" name="FL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom name="fl_calf1" class="calf1"/> 
            <!-- removed: <geom name="fl_calf2" class="calf2"/> (simplified to single capsule) --> 
            <geom name="FL_foot" class="foot"/>
            <site name="FL" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
          </body>
        </body>
      </body>
      <body name="RR_hip" pos="-0.1881 -0.04675 0">
        <inertial pos="0.0049166 0.00762615 -8.865e-05" quat="0.495027 0.482891 0.507341 0.514169" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
        <joint class="abduction" name="RR_hip_joint"/>
        <geom class="visual" quat="0 0 0 -1" mesh="hip"/>
        <geom name="rr_hip" class="hip_right1"/> 
        <body name="RR_thigh" pos="0 -0.08 0">
          <inertial pos="-0.00304722 0.019315 -0.0305004" quat="0.65243 -0.0272313 0.0775126 0.753383" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
          <joint class="hip" name="RR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom name="rr_thigh1" class="thigh1"/>
          <!-- removed: <geom name="rr_thigh2" class="thigh2"/> (simplified to single capsule) -->
          <!-- removed: <geom name="rr_thigh3" class="thigh3"/> (simplified to single capsule) --> 
          <body name="RR_calf" pos="0 0 -0.213">
            <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
            <joint class="knee" name="RR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom name="rr_calf1" class="calf1"/>
            <!-- removed: <geom name="rr_calf2" class="calf2"/> (simplified to single capsule) --> 
            <geom name="RR_foot" class="foot"/>
            <site name="RR" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
          </body>
        </body>
      </body>
      <body name="RL_hip" pos="-0.1881 0.04675 0">
        <inertial pos="0.0049166 -0.00762615 -8.865e-05" quat="0.514169 0.507341 0.482891 0.495027" mass="0.68" diaginertia="0.000734064 0.000468438 0.000398719"/>
        <joint class="abduction" name="RL_hip_joint"/>
        <geom class="visual" quat="0 0 1 0" mesh="hip"/>
        <geom name="rl_hip" class="hip_left1"/> 
        <body name="RL_thigh" pos="0 0.08 0">
          <inertial pos="-0.00304722 -0.019315 -0.0305004" quat="0.753383 0.0775126 -0.0272313 0.65243" mass="1.009" diaginertia="0.00478717 0.00460903 0.000709268"/>
          <joint class="hip" name="RL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom name="rl_thigh1" class="thigh1"/>
          <!-- removed: <geom name="rl_thigh2" class="thigh2"/> (simplified to single capsule) -->
          <!-- removed: <geom name="rl_thigh3" class="thigh3"/> (simplified to single capsule) --> 
          <body name="RL_calf" pos="0 0 -0.213">
            <inertial pos="0.00429862 0.000976676 -0.146197" quat="0.691246 0.00357467 0.00511118 0.722592" mass="0.195862" diaginertia="0.00149767 0.00148468 3.58427e-05"/>
            <joint class="knee" name="RL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom name="rl_calf1" class="calf1"/>
            <!-- removed: <geom name="rl_calf2" class="calf2"/> (simplified to single capsule) --> 
            <geom name="RL_foot" class="foot"/>
            <site name="RL" pos="0 0 -0.213" type="sphere" size="0.023" group="5"/>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <motor class="abduction" name="FR_hip" joint="FR_hip_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="hip" name="FR_thigh" joint="FR_thigh_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="knee" name="FR_calf" joint="FR_calf_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="abduction" name="FL_hip" joint="FL_hip_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="hip" name="FL_thigh" joint="FL_thigh_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="knee" name="FL_calf" joint="FL_calf_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="abduction" name="RR_hip" joint="RR_hip_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="hip" name="RR_thigh" joint="RR_thigh_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="knee" name="RR_calf" joint="RR_calf_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="abduction" name="RL_hip" joint="RL_hip_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="hip" name="RL_thigh" joint="RL_thigh_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>
    <motor class="knee" name="RL_calf" joint="RL_calf_joint" ctrllimited="true" ctrlrange="-23.7 23.7"/>

    <!-- <position class="abduction" name="FR_hip" joint="FR_hip_joint" />
    <position class="hip" name="FR_thigh" joint="FR_thigh_joint" />
    <position class="knee" name="FR_calf" joint="FR_calf_joint" />
    <position class="abduction" name="FL_hip" joint="FL_hip_joint" />
    <position class="hip" name="FL_thigh" joint="FL_thigh_joint" />
    <position class="knee" name="FL_calf" joint="FL_calf_joint" />
    <position class="abduction" name="RR_hip" joint="RR_hip_joint" />
    <position class="hip" name="RR_thigh" joint="RR_thigh_joint" />
    <position class="knee" name="RR_calf" joint="RR_calf_joint" />
    <position class="abduction" name="RL_hip" joint="RL_hip_joint" />
    <position class="hip" name="RL_thigh" joint="RL_thigh_joint" />
    <position class="knee" name="RL_calf" joint="RL_calf_joint" /> -->
  </actuator>

  <sensor>
    <gyro site="imu" name="gyro"/>
    <velocimeter site="imu" name="local_linvel"/>
    <framepos objtype="site" objname="imu" name="position"/>
    <!-- <framezaxis objtype="site" objname="imu" name="upvector"/>
    <framexaxis objtype="site" objname="imu" name="forwardvector"/> -->
    <framelinvel objtype="site" objname="imu" name="global_linvel"/>
    <frameangvel objtype="site" objname="imu" name="global_angvel"/>
    <framequat objtype="site" objname="imu" name="orientation"/>
    <framelinvel objtype="site" objname="FR" name="FR_global_linvel"/>
    <framelinvel objtype="site" objname="FL" name="FL_global_linvel"/>
    <framelinvel objtype="site" objname="RR" name="RR_global_linvel"/>
    <framelinvel objtype="site" objname="RL" name="RL_global_linvel"/>
    <framepos objtype="site" objname="FR" name="FR_pos" reftype="site" refname="imu"/>
    <framepos objtype="site" objname="FL" name="FL_pos" reftype="site" refname="imu"/>
    <framepos objtype="site" objname="RR" name="RR_pos" reftype="site" refname="imu"/>
    <framepos objtype="site" objname="RL" name="RL_pos" reftype="site" refname="imu"/>
    <framepos objtype="site" objname="head" name="head_pos"/>
  </sensor>
</mujoco>
