<?xml version='1.0' encoding='UTF-8'?>
<!-- Cassie simulation model developed by the Dynamic Robotics Laboratory -->
<mujoco model="cassie">
  <compiler inertiafromgeom="auto" angle="degree" eulerseq="zyx" meshdir="cassie-stl-meshes"/>
  <size nuser_actuator="1" nuser_sensor="1" nuser_geom="1"/>
  <option timestep="0.0005" iterations="50" solver="PGS" gravity="0 0 -9.81"/>
  <!-- Timestep is set to 0.0005 because our controller runs at 2 kHz -->
  <!-- Larger values still have stable dynamics -->
  <visual>
    <quality shadowsize="4096"/>
    <map znear="0.05"/>
  </visual>
  <default>
    <joint limited="true"/>
    <motor ctrllimited="true"/>
    <!-- <geom contype='0' conaffinity='0' condim='1' solref='0.005 1' friction='.4 0.0001 0.00001'/> -->
    <geom contype="0" conaffinity="0" condim="1" solref="0.005 1"/>
    <equality solref="0.005 1"/>
    <default class="cassie">
      <geom material="cassie"/>
    </default>
    <default class="collision">
      <!-- User parameter for geoms is used to identify collision types -->
      <geom material="collision" contype="1" user="2"/>
      <default class="collision-left">
        <geom contype="2" conaffinity="4"/>
      </default>
      <default class="collision-right">
        <geom contype="4" conaffinity="2"/>
      </default>
    </default>
    <default class="obstacle">
      <geom material="obstacle" condim="3" contype="8" conaffinity="15" user="1"/>
    </default>
  </default>
  <asset>
    <texture type="skybox" builtin="gradient" width="128" height="128" rgb1="0.4 0.6 0.8" rgb2="0 0 0"/>
    <texture name="plane" type="2d" builtin="checker" rgb1="0.2 0.2 0.2" rgb2="0.3 0.3 0.3" width="512" height="512"/>
    <material name="plane" reflectance="0.0" texture="plane" texrepeat="1 1" texuniform="true"/>
    <material name="cassie" rgba="0.9 0.5 0.1 1"/>
    <material name="collision" rgba="0.1 0.5 0.9 0"/>
    <material name="obstacle" rgba="0.2 0.6 0.2 1"/>
    <mesh name="cassie-pelvis" file="pelvis.stl"/>
    <mesh name="left-hip-roll" file="hip-roll.stl"/>
    <mesh name="left-hip-yaw" file="hip-yaw.stl"/>
    <mesh name="left-hip-pitch" file="hip-pitch.stl"/>
    <mesh name="left-achilles-rod" file="achilles-rod.stl"/>
    <mesh name="left-knee" file="knee.stl"/>
    <mesh name="left-knee-spring" file="knee-spring.stl"/>
    <mesh name="left-shin" file="shin.stl"/>
    <mesh name="left-tarsus" file="tarsus.stl"/>
    <mesh name="left-heel-spring" file="heel-spring.stl"/>
    <mesh name="left-foot-crank" file="foot-crank.stl"/>
    <mesh name="left-plantar-rod" file="plantar-rod.stl"/>
    <mesh name="left-foot" file="foot.stl"/>
    <mesh name="right-hip-roll" scale="1 -1 1" file="hip-roll.stl"/>
    <mesh name="right-hip-yaw" scale="1 -1 1" file="hip-yaw.stl"/>
    <mesh name="right-hip-pitch" scale="1 1 -1" file="hip-pitch.stl"/>
    <mesh name="right-achilles-rod" scale="1 1 -1" file="achilles-rod.stl"/>
    <mesh name="right-knee" scale="1 1 -1" file="knee.stl"/>
    <mesh name="right-knee-spring" scale="1 1 -1" file="knee-spring.stl"/>
    <mesh name="right-shin" scale="1 1 -1" file="shin.stl"/>
    <mesh name="right-tarsus" scale="1 1 -1" file="tarsus.stl"/>
    <mesh name="right-heel-spring" scale="1 1 -1" file="heel-spring.stl"/>
    <mesh name="right-foot-crank" scale="1 1 -1" file="foot-crank.stl"/>
    <mesh name="right-plantar-rod" scale="1 1 -1" file="plantar-rod.stl"/>
    <mesh name="right-foot" scale="1 1 -1" file="foot.stl"/>
  </asset>
  <worldbody>
    <geom name="floor" pos="0 0 -0.01" size="100 5 .125" type="plane" material="plane" condim="3" conaffinity="15" priority="1"/>
    <light mode="trackcom" pos="0 0 5" dir="0 0 -1"/>
    <light directional="true" pos="0 0 3" dir="0 0 -1" diffuse="0.2 0.2 0.2" specular="0 0 0"/>
    <body name="cassie-pelvis" pos="0 0 1.01" childclass="cassie">
      <camera name="track" pos="0 -3 1" zaxis="0 -1 0.5" mode="track"/>
      <!--camera name='track' pos='-3 0 0' xyaxes='0 -1 0 0 0 1' mode='track'/-->
      <inertial pos="0.05066 0.000346 0.02841" mass="10.33" fullinertia="0.085821 0.049222 0.08626 1.276e-05 -0.00016022 -0.000414"/>
      <joint type="slide" axis="1 0 0" limited="false"/>
      <joint type="slide" axis="0 1 0" limited="false"/>
      <joint type="slide" axis="0 0 1" limited="false" ref="1.01"/>
      <joint type="ball" limited="false"/>
      <geom type="mesh" mesh="cassie-pelvis"/>
      <geom type="sphere" size="0.15" pos="0.02 0 0.02" class="collision"/>
      <site name="imu" size="0.01" pos="0.03155 0 -0.07996"/>
      <body name="left-hip-roll" pos="0.021 0.135 0" xyaxes="0 0 -1 0 1 0">
        <inertial pos="-0.01793 0.0001 -0.04428" mass="1.82" fullinertia="0.003431 0.003793 0.002135 -6.65e-07 -0.00084 3.99e-06"/>
        <joint name="left-hip-roll" type="hinge" range="-15 22.5" damping="1" armature="0.038125"/>
        <geom type="mesh" mesh="left-hip-roll"/>
        <body name="left-hip-yaw" pos="0 0 -0.07" xyaxes="0 0 1 0 1 0">
          <inertial pos="0 -1e-05 -0.034277" mass="1.171" fullinertia="0.002443 0.002803 0.000842 -4e-08 2.462e-07 -2.71e-08"/>
          <joint name="left-hip-yaw" type="hinge" range="-22.5 22.5" damping="1" armature="0.038125"/>
          <geom type="mesh" mesh="left-hip-yaw"/>
          <body name="left-hip-pitch" pos="0 0 -0.09" xyaxes="0 0 -1 1 0 0">
            <inertial pos="0.05946 5e-05 -0.03581" mass="5.52" fullinertia="0.010898 0.029714 0.030257 -0.0002669 -5.721e-05 9.17e-06"/>
            <joint name="left-hip-pitch" type="hinge" range="-50 80" damping="1" armature="0.09344"/>
            <geom type="mesh" mesh="left-hip-pitch"/>
            <geom type="capsule" size="0.08" fromto="0 0 -0.04 0.12 0 -0.0355" class="collision"/>
            <body name="left-achilles-rod" pos="0 0 0.045" xyaxes="0.7922 -0.60599 -0.072096 0.60349 0.79547 -0.054922">
              <inertial pos="0.24719 0 0" mass="0.1567" fullinertia="3.754e-06 0.004487 0.004488 -3.74e-08 -1.61e-08 0"/>
              <joint name="left-achilles-rod" type="ball" limited="false" damping="0.01"/>
              <geom type="mesh" mesh="left-achilles-rod"/>
            </body>
            <body name="left-knee" pos="0.12 0 0.0045" xyaxes="0.70711 -0.70711 0 0.70711 0.70711 0">
              <inertial pos="0.023 0.03207 -0.002181" mass="0.7578" fullinertia="0.001376 0.0010335 0.0021637 -0.00039744 -4.085e-05 -5.374e-05"/>
              <joint name="left-knee" type="hinge" ref="-45" range="-164 -37" damping="1" armature="0.09344"/>
              <geom type="mesh" mesh="left-knee"/>
              <body name="left-knee-spring" pos="0.06068 0.08241 0">
                <inertial pos="0.0836 0.0034 0" mass="0.186" fullinertia="5.215e-05 0.00041205 0.0003669 6.87e-06 0 0"/>
                <geom type="mesh" mesh="left-knee-spring"/>
              </body>
              <body name="left-shin" pos="0.06068 0.04741 0">
                <inertial pos="0.18338 0.001169 0.0002123" mass="0.577" fullinertia="0.00035939 0.014728 0.014707 -0.00020981 2.266e-05 -1.2e-07"/>
                <joint name="left-shin" type="hinge" range="-20 20" stiffness="1500" damping="0.1"/>
                <geom type="mesh" mesh="left-shin"/>
                <geom type="capsule" size="0.04" fromto="0 0 0 0.43476 0.02 0" class="collision-left"/>
                <body name="left-tarsus" pos="0.43476 0.02 0" xyaxes="0.52992 0.84805 0 -0.84805 0.52992 0">
                  <inertial pos="0.11046 -0.03058 -0.00131" mass="0.782" fullinertia="0.00039238 0.013595 0.013674 0.00023651 -4.987e-05 -4.82e-06"/>
                  <joint name="left-tarsus" type="hinge" ref="58" range="50 170" damping="0.1"/>
                  <geom type="mesh" mesh="left-tarsus"/>
                  <geom type="capsule" size="0.04" fromto="-0.01269 -0.03059 0.00092 0.408 -0.04 0" class="collision-left"/>
                  <body name="left-heel-spring" pos="-0.01269 -0.03059 0.00092" xyaxes="-0.91211 0.40829 0.036948 -0.40992 -0.90952 -0.068841">
                    <inertial pos="0.081 0.0022 0" mass="0.126" fullinertia="2.959e-05 0.00022231 0.0002007 7.15e-06 -6e-07 1e-07"/>
                    <joint name="left-heel-spring" type="hinge" limited="false" stiffness="1250"/>
                    <geom type="mesh" mesh="left-heel-spring"/>
                  </body>
                  <body name="left-foot-crank" pos="0.058 -0.034 0.02275">
                    <inertial pos="0.00493 2e-05 -0.00215" mass="0.1261" fullinertia="2.6941e-05 4.9621e-05 6.3362e-05 -2.1e-09 -3.9623e-06 -1.09e-08"/>
                    <joint name="left-foot-crank" type="hinge" range="-140 -30" damping="1"/>
                    <geom type="mesh" mesh="left-foot-crank"/>
                    <body name="left-plantar-rod" pos="0.055 0 -0.00791">
                      <inertial pos="0.17792 0 0" mass="0.1186" fullinertia="2.779e-06 0.001774 0.001775 -2.34e-08 -8.1e-09 0"/>
                      <joint name="left-plantar-rod" type="hinge" limited="false"/>
                      <geom type="mesh" mesh="left-plantar-rod"/>
                    </body>
                  </body>
                  <body name="left-foot" pos="0.408 -0.04 0">
                    <inertial pos="0.00474 0.02748 -0.00014" mass="0.1498" fullinertia="0.00017388 0.00016793 0.00033261 0.00011814 1.36e-06 -4e-07"/>
                    <joint name="left-foot" type="hinge" range="-140 -30" damping="1" armature="0.01225"/>
                    <geom type="mesh" mesh="left-foot"/>
                    <geom type="capsule" size="0.02" fromto="-0.052821 0.092622 0 0.069746 -0.010224 0" class="collision-left"/>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
      <body name="right-hip-roll" pos="0.021 -0.135 0" xyaxes="0 0 -1 0 1 0">
        <inertial pos="-0.01793 0.0001 -0.04428" mass="1.82" fullinertia="0.003431 0.003793 0.002135 6.65e-07 -0.00084 -3.99e-06"/>
        <joint name="right-hip-roll" type="hinge" range="-22.5 15" damping="1" armature="0.038125"/>
        <geom type="mesh" mesh="right-hip-roll"/>
        <body name="right-hip-yaw" pos="0 0 -0.07" xyaxes="0 0 1 0 1 0">
          <inertial pos="0 1e-05 -0.034277" mass="1.171" fullinertia="0.002443 0.002803 0.000842 4e-08 2.462e-07 2.71e-08"/>
          <joint name="right-hip-yaw" type="hinge" range="-22.5 22.5" damping="1" armature="0.038125"/>
          <geom type="mesh" mesh="right-hip-yaw"/>
          <body name="right-hip-pitch" pos="0 0 -0.09" xyaxes="0 0 -1 1 0 0">
            <inertial pos="0.05946 5e-05 0.03581" mass="5.52" fullinertia="0.010898 0.029714 0.030257 -0.0002669 5.721e-05 -9.17e-06"/>
            <joint name="right-hip-pitch" type="hinge" range="-50 80" damping="1" armature="0.09344"/>
            <geom type="mesh" mesh="right-hip-pitch"/>
            <geom type="capsule" size="0.08" fromto="0 0 0.04 0.12 0 0.0355" class="collision"/>
            <body name="right-achilles-rod" pos="0 0 -0.045" xyaxes="0.7922 -0.60599 0.072096 0.60349 0.79547 0.054922">
              <inertial pos="0.24719 0 0" mass="0.1567" fullinertia="3.754e-06 0.004487 0.004488 -3.74e-08 1.61e-08 0"/>
              <joint name="right-achilles-rod" type="ball" limited="false" damping="0.01"/>
              <geom type="mesh" mesh="right-achilles-rod"/>
            </body>
            <body name="right-knee" pos="0.12 0 -0.0045" xyaxes="0.70711 -0.70711 0 0.70711 0.70711 0">
              <inertial pos="0.023 0.03207 0.002181" mass="0.7578" fullinertia="0.001376 0.0010335 0.0021637 -0.00039744 4.085e-05 5.374e-05"/>
              <joint name="right-knee" type="hinge" ref="-45" range="-164 -37" damping="1" armature="0.09344"/>
              <geom type="mesh" mesh="right-knee"/>
              <body name="right-knee-spring" pos="0.06068 0.08241 0">
                <inertial pos="0.0836 0.0034 0" mass="0.186" fullinertia="5.215e-05 0.00041205 0.0003669 6.87e-06 0 0"/>
                <geom type="mesh" mesh="right-knee-spring"/>
              </body>
              <body name="right-shin" pos="0.06068 0.04741 0">
                <inertial pos="0.18338 0.001169 -0.0002123" mass="0.577" fullinertia="0.00035939 0.014728 0.014707 -0.00020981 -2.266e-05 1.2e-07"/>
                <joint name="right-shin" type="hinge" range="-20 20" stiffness="1500" damping="0.1"/>
                <geom type="mesh" mesh="right-shin"/>
                <geom type="capsule" size="0.04" fromto="0 0 0 0.43476 0.02 0" class="collision-right"/>
                <body name="right-tarsus" pos="0.43476 0.02 0" xyaxes="0.52992 0.84805 0 -0.84805 0.52992 0">
                  <inertial pos="0.11046 -0.03058 0.00131" mass="0.782" fullinertia="0.00039238 0.013595 0.013674 0.00023651 4.987e-05 4.82e-06"/>
                  <joint name="right-tarsus" type="hinge" ref="58" range="50 170" damping="0.1"/>
                  <geom type="mesh" mesh="right-tarsus"/>
                  <geom type="capsule" size="0.04" fromto="-0.01269 -0.03059 -0.00092 0.408 -0.04 0" class="collision-right"/>
                  <body name="right-heel-spring" pos="-0.01269 -0.03059 -0.00092" xyaxes="-0.91211 0.40829 -0.036948 -0.40992 -0.90952 0.068841">
                    <inertial pos="0.081 0.0022 0" mass="0.126" fullinertia="2.959e-05 0.00022231 0.0002007 7.15e-06 6e-07 -1e-07"/>
                    <joint name="right-heel-spring" type="hinge" limited="false" stiffness="1250"/>
                    <geom type="mesh" mesh="right-heel-spring"/>
                  </body>
                  <body name="right-foot-crank" pos="0.058 -0.034 -0.02275">
                    <inertial pos="0.00493 2e-05 0.00215" mass="0.1261" fullinertia="2.6941e-05 4.9621e-05 6.3362e-05 -2.1e-09 3.9623e-06 1.09e-08"/>
                    <joint name="right-foot-crank" type="hinge" range="-140 -30" damping="1"/>
                    <geom type="mesh" mesh="right-foot-crank"/>
                    <body name="right-plantar-rod" pos="0.055 0 0.00791">
                      <inertial pos="0.17792 0 0" mass="0.1186" fullinertia="2.779e-06 0.001774 0.001775 -2.34e-08 8.1e-09 0"/>
                      <joint name="right-plantar-rod" type="hinge" limited="false"/>
                      <geom type="mesh" mesh="right-plantar-rod"/>
                    </body>
                  </body>
                  <body name="right-foot" pos="0.408 -0.04 0">
                    <inertial pos="0.00474 0.02748 0.00014" mass="0.1498" fullinertia="0.00017388 0.00016793 0.00033261 0.00011814 -1.36e-06 4e-07"/>
                    <joint name="right-foot" type="hinge" range="-140 -30" damping="1" armature="0.01225"/>
                    <geom type="mesh" mesh="right-foot"/>
                    <geom type="capsule" size="0.02" fromto="-0.052821 0.092622 0 0.069746 -0.010224 0" class="collision-right"/>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
    <!-- Add any other moving bodies after the Cassie definition -->
    <!-- <body name='box' pos='2 0 0.2'> -->
    <!--   <freejoint/> -->
    <!--   <geom type='box' size='0.1 0.1 0.1' mass='3' class='obstacle'/> -->
    <!-- </body> -->
    <geom name="waypoint1" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint2" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint3" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint4" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint5" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint6" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint7" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint8" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint9" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint10" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint11" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint12" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint13" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint14" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint15" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint16" pos="0.0 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint17" pos="0.001402125706671642 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint18" pos="0.011804766336572903 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint19" pos="0.031739464320212685 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint20" pos="0.06048254324099345 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint21" pos="0.09731032668231832 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint22" pos="0.14149913822758986 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint23" pos="0.19232530146021087 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint24" pos="0.2490651399635841 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint25" pos="0.3109949773211127 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint26" pos="0.3773911371161989 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint27" pos="0.4475299429322461 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint28" pos="0.5206877183526566 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint29" pos="0.5961407869608335 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint30" pos="0.6731654723401793 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint31" pos="0.7510380980740973 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint32" pos="0.8290349877459898 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint33" pos="0.9064324649392596 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint34" pos="0.9825114791111614 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint35" pos="1.061053144757774 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint36" pos="1.1455714252417715 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint37" pos="1.2357539868310978 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint38" pos="1.3312884957936977 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint39" pos="1.4318626183975143 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint40" pos="1.5371640209104942 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint41" pos="1.64688036960058 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint42" pos="1.7606993307357173 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint43" pos="1.8783085705838527 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint44" pos="1.9993957554129276 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint45" pos="2.1236485514908874 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint46" pos="2.2507546250856767 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint47" pos="2.380401642465241 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint48" pos="2.5122772698975253 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint49" pos="2.646069173650472 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint50" pos="2.781465019992028 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint51" pos="2.918152475190135 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint52" pos="3.056499455033961 -5.813304999691127e-05 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint53" pos="3.1995909165562777 -0.00044371880590077234 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint54" pos="3.347655315214637 -0.0011522789453284332 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint55" pos="3.5004687162867274 -0.002138903936389269 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint56" pos="3.6578071850502374 -0.0033586842471873264 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint57" pos="3.819446786782863 -0.004766710345833758 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint58" pos="3.9851635867622903 -0.006318072700439713 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint59" pos="4.1547336502662136 -0.007967861779107466 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint60" pos="4.327933042572319 -0.009671168049944612 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint61" pos="4.504537828958302 -0.011383081981062304 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint62" pos="4.684324074701848 -0.013058694040566364 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint63" pos="4.867067845080656 -0.01465309469656795 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint64" pos="5.0525452053724065 -0.016121374417171097 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint65" pos="5.240532220854796 -0.017418623670485186 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint66" pos="5.430804956805518 -0.01849993292461427 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint67" pos="5.623139478502253 -0.0193203926476766 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint68" pos="5.8173118512227004 -0.019835093307769117 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint69" pos="6.013104097146328 -0.019999129808516255 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint70" pos="6.211825211885194 -0.019785747157676777 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint71" pos="6.414535269124362 -0.01922768384956797 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint72" pos="6.621146569097501 -0.01836984941607866 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint73" pos="6.8315714120382856 -0.017257153389101276 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint74" pos="7.045722098180381 -0.0159345053005282 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint75" pos="7.263510927757459 -0.01444681468225184 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint76" pos="7.484850201003191 -0.012838991066164596 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint77" pos="7.709652218151239 -0.011155943984158867 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint78" pos="7.937829279435278 -0.009442582968121727 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint79" pos="8.169293685088977 -0.007743817549954458 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint80" pos="8.403957735346006 -0.006104557261542354 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint81" pos="8.641733730440032 -0.0045697116347760405 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint82" pos="8.882533970604726 -0.003184190201556803 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint83" pos="9.126270756073756 -0.00199290249376638 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint84" pos="9.372856387080793 -0.0010407580433042798 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint85" pos="9.622203163859508 -0.0003726663820593501 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint86" pos="9.874223386643568 -3.3537041923992206e-05 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint87" pos="10.129124909411903 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint88" pos="10.388118243437857 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint89" pos="10.651283738808496 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint90" pos="10.918551467100823 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint91" pos="11.189851499891848 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint92" pos="11.465113908758568 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint93" pos="11.744268765277994 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint94" pos="12.027246141027119 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint95" pos="12.313976107582954 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint96" pos="12.604388736522512 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint97" pos="12.898414099422775 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint98" pos="13.195982267860767 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint99" pos="13.497023313413484 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint100" pos="13.801467307657928 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint101" pos="14.109244322171106 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint102" pos="14.420284428530016 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
    <geom name="waypoint103" pos="14.734517698311674 0.0 1.01 " size="0.03 0.03 0.03" type="sphere" contype="0" rgba="1 0.9 0 0.7"/>
  </worldbody>
  <equality>
    <connect body1="left-plantar-rod" body2="left-foot" anchor="0.35012 0 0"/>
    <connect body1="left-achilles-rod" body2="left-heel-spring" anchor="0.5012 0 0"/>
    <connect body1="right-plantar-rod" body2="right-foot" anchor="0.35012 0 0"/>
    <connect body1="right-achilles-rod" body2="right-heel-spring" anchor="0.5012 0 0"/>
  </equality>
  <actuator>
    <!-- User parameter is the maximum no-load motor RPM -->
    <motor name="left-hip-roll" joint="left-hip-roll" gear="25" ctrllimited="true" ctrlrange="-4.5 4.5" user="2900"/>
    <motor name="left-hip-yaw" joint="left-hip-yaw" gear="25" ctrllimited="true" ctrlrange="-4.5 4.5" user="2900"/>
    <motor name="left-hip-pitch" joint="left-hip-pitch" gear="16" ctrllimited="true" ctrlrange="-12.2 12.2" user="1300"/>
    <motor name="left-knee" joint="left-knee" gear="16" ctrllimited="true" ctrlrange="-12.2 12.2" user="1300"/>
    <motor name="left-foot" joint="left-foot" gear="50" ctrllimited="true" ctrlrange="-0.9 0.9" user="5500"/>
    <motor name="right-hip-roll" joint="right-hip-roll" gear="25" ctrllimited="true" ctrlrange="-4.5 4.5" user="2900"/>
    <motor name="right-hip-yaw" joint="right-hip-yaw" gear="25" ctrllimited="true" ctrlrange="-4.5 4.5" user="2900"/>
    <motor name="right-hip-pitch" joint="right-hip-pitch" gear="16" ctrllimited="true" ctrlrange="-12.2 12.2" user="1300"/>
    <motor name="right-knee" joint="right-knee" gear="16" ctrllimited="true" ctrlrange="-12.2 12.2" user="1300"/>
    <motor name="right-foot" joint="right-foot" gear="50" ctrllimited="true" ctrlrange="-0.9 0.9" user="5500"/>
  </actuator>
  <sensor>
    <!-- User parameter is the number of absolute encoder bits -->
    <actuatorpos name="left-hip-roll-input" actuator="left-hip-roll" user="13"/>
    <actuatorpos name="left-hip-yaw-input" actuator="left-hip-yaw" user="13"/>
    <actuatorpos name="left-hip-pitch-input" actuator="left-hip-pitch" user="13"/>
    <actuatorpos name="left-knee-input" actuator="left-knee" user="13"/>
    <actuatorpos name="left-foot-input" actuator="left-foot" user="18"/>
    <jointpos name="left-shin-output" joint="left-shin" user="18" noise="2e-4"/>
    <jointpos name="left-tarsus-output" joint="left-tarsus" user="18" noise="2e-4"/>
    <jointpos name="left-foot-output" joint="left-foot" user="13"/>
    <actuatorpos name="right-hip-roll-input" actuator="right-hip-roll" user="13"/>
    <actuatorpos name="right-hip-yaw-input" actuator="right-hip-yaw" user="13"/>
    <actuatorpos name="right-hip-pitch-input" actuator="right-hip-pitch" user="13"/>
    <actuatorpos name="right-knee-input" actuator="right-knee" user="13"/>
    <actuatorpos name="right-foot-input" actuator="right-foot" user="18"/>
    <jointpos name="right-shin-output" joint="right-shin" user="18" noise="2e-4"/>
    <jointpos name="right-tarsus-output" joint="right-tarsus" user="18" noise="2e-4"/>
    <jointpos name="right-foot-output" joint="right-foot" user="13"/>
    <framequat name="pelvis-orientation" objtype="site" objname="imu"/>
    <gyro name="pelvis-angular-velocity" site="imu" noise="5e-4" cutoff="34.9"/>
    <accelerometer name="pelvis-linear-acceleration" site="imu" noise="1e-2" cutoff="157"/>
    <magnetometer name="pelvis-magnetometer" site="imu"/>
  </sensor>
</mujoco>
