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

  <option cone="elliptic" impratio="100" iterations="60"/>

  <default>
    <material reflectance="0.5" metallic="0.0" roughness="0.7"/>
    <default class="anymal_c">
      <joint damping="1" frictionloss="0.1" />

      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" />
        <default class="visual_zflip">
          <geom quat="0 0 0 1" />
        </default>
      </default>
      <default class="collision">
        <geom group="3" type="cylinder" contype="0" conaffinity="0"/>
        <default class="foot">
          <geom type="sphere" size="0.03" pos="0 0 0.02325" priority="1" solimp="0.015 1 0.03" contype="0" conaffinity="3"
            condim="6"
            friction="0.8 0.02 0.01" />
        </default>
      </default>

      <default class="affine">
        <position kp="200" kv="1" ctrlrange="-6.28 6.28" forcerange="-140 140" />
      </default>
    </default>
  </default>

  <asset>
    <material name="black_plastic" rgba="0.007 0.007 0.007 1" />
    <material name="green" rgba="0.052 0.178 0.033 1" />
    <material name="red" rgba="0.178 0.006 0.005 1" />
    <material name="yellow" rgba="0.178 0.119 0.006 1" />
    <material name="lwl" rgba="0.8 0.8 0.8 1" />
    <material name="base" texture="base" />
    <material name="top_shell" texture="top_shell" />
    <material name="bottom_shell" texture="bottom_shell" />
    <material name="hip_l" texture="hip_l" />
    <material name="thigh" texture="thigh" />
    <material name="shank_l" texture="shank_l" metallic="1.0"/>
    <material name="foot" texture="foot" />
    <material name="hip_r" texture="hip_r" />
    <material name="shank" texture="shank_r" metallic="1.0"/>
    <material name="hatch" texture="hatch" />
    <material name="remote" texture="remote" />
    <material name="handle" texture="handle" />
    <material name="face" texture="face" />
    <material name="depth_camera" texture="depth_camera" />
    <material name="wide_angle_camera" texture="wide_angle_camera" />
    <material name="battery" texture="battery" />
    <material name="lidar_cage" texture="lidar_cage" />
    <material name="lidar" texture="lidar" />
    <material name="drive" texture="drive" />

    <texture type="2d" name="base" file="assets/base.png" />
    <texture type="2d" name="top_shell" file="assets/top_shell.png" />
    <texture type="2d" name="bottom_shell" file="assets/bottom_shell.png" />
    <texture type="2d" name="remote" file="assets/remote.png" />
    <texture type="2d" name="hatch" file="assets/hatch.png" />
    <texture type="2d" name="handle" file="assets/handle.png" />
    <texture type="2d" name="face" file="assets/face.png" />
    <texture type="2d" name="battery" file="assets/battery.png" />
    <texture type="2d" name="lidar_cage" file="assets/lidar_cage.png" />
    <texture type="2d" name="depth_camera" file="assets/depth_camera.png" />
    <texture type="2d" name="wide_angle_camera" file="assets/wide_angle_camera.png" />
    <texture type="2d" name="lidar" file="assets/lidar.png" />
    <texture type="2d" name="drive" file="assets/drive.png" />
    <texture type="2d" name="hip_l" file="assets/hip_l.png" />
    <texture type="2d" name="shank_l" file="assets/shank_l.png" />
    <texture type="2d" name="foot" file="assets/foot.png" />
    <texture type="2d" name="thigh" file="assets/thigh.png" />
    <texture type="2d" name="shank_r" file="assets/shank_r.png" />
    <texture type="2d" name="hip_r" file="assets/hip_r.png" />

    <mesh file="assets/base_0.obj" />
    <mesh file="assets/base_1.obj" />
    <mesh file="assets/base_2.obj" />
    <mesh file="assets/base_3.obj" />
    <mesh file="assets/base_4.obj" />
    <mesh file="assets/base_5.obj" />
    <mesh file="assets/top_shell.obj" />
    <mesh file="assets/bottom_shell.obj" />
    <mesh file="assets/remote.obj" />
    <mesh file="assets/handle.obj" />
    <mesh file="assets/face.obj" />
    <mesh file="assets/wide_angle_camera.obj" />
    <mesh file="assets/depth_camera.obj" />
    <mesh file="assets/battery.obj" />
    <mesh file="assets/lidar_cage.obj" />
    <mesh file="assets/lidar.obj" />
    <mesh file="assets/drive.obj" />
    <mesh file="assets/hip_l.obj" />
    <mesh file="assets/thigh.obj" />
    <mesh file="assets/shank_l.obj" />
    <mesh file="assets/foot.obj" />
    <mesh file="assets/hip_r.obj" />
    <mesh file="assets/hatch.obj" />
    <mesh file="assets/shank_r.obj" />

    <!-- Target arrow mesh -->
    <mesh name="arrow_cone" file="assets/cone.stl" scale="0.003 0.003 0.003" />
    <mesh name="arrow" file="assets/arrow.obj"/>

    <!-- Target arrow material -->
    <material name="target_green" rgba="0 1 0 0.9" />
  </asset>

  <worldbody>

    <!-- Target position marker: 3D arrow (cylinder + cone) -->
    <body name="target_marker" pos="0 0 0.05" mocap="true">
      <!-- Arrow head: cone, base attaches to cylinder end face, tip points forward -->
      <!-- Cylinder end at x=0.3, cone base also near x=0.3 -->
      <!-- Cone default orientation: tip points to +Z, need to rotate -90° around Y axis to make tip point to +X -->
      <geom name="arrow_head" type="mesh" mesh="arrow"
        pos="0.0 0.0 0.09" quat="1 0 0 0"
        material="target_green" contype="0" conaffinity="0" />
    </body>
    <body name="base" pos="0 0 0.56" quat="1 0 0 0" childclass="anymal_c">
      <freejoint />
      <inertial mass="19.2035" pos="0.0025 0 0.0502071" quat="0.5 0.5 0.5 0.5"
        diaginertia="0.639559 0.624031 0.217374" />

      <!-- IMU sensor position -->
      <site name="imu_site" pos="0 0 0" size="0.01" />

      <geom mesh="base_0" material="green" class="visual" />
      <geom mesh="base_1" material="yellow" class="visual" />
      <geom mesh="base_2" material="red" class="visual" />
      <geom mesh="base_3" material="black_plastic" class="visual" />
      <geom mesh="base_4" material="lwl" class="visual" />
      <geom mesh="base_5" material="base" class="visual" />
      <geom name="base" class="collision" size="0.29 0.07 0.09" type="box" />
      <geom class="collision" size="0.09 0.0725" pos="0.2175 0.07 0" quat="1 0 1 0" />
      <geom class="collision" size="0.09 0.0725" pos="-0.2175 0.07 0" quat="1 0 1 0" />
      <geom class="collision" size="0.09 0.0725" pos="0.2175 -0.07 0" quat="1 0 1 0" />
      <geom class="collision" size="0.09 0.0725" pos="-0.2175 -0.07 0" quat="1 0 1 0" />
      <geom material="top_shell" mesh="top_shell" class="visual" />
      <geom material="bottom_shell" mesh="bottom_shell" class="visual" />
      <geom material="remote" mesh="remote" class="visual" />
      <geom material="handle" mesh="handle" class="visual" />
      <geom pos="0.4145 0 0" material="face" mesh="face" class="visual" />
      <geom class="collision" size="0.055 0.07 0.09" pos="0.4695 0 0" type="box" />
      <geom class="collision" size="0.09 0.055" pos="0.4695 0.07 0" quat="1 0 1 0" />
      <geom class="collision" size="0.09 0.055" pos="0.4695 -0.07 0" quat="1 0 1 0" />
      <geom pos="0.46165 0 -0.0292" quat="0.965926 0 0.258819 0" material="depth_camera"
        mesh="depth_camera"
        class="visual" />
      <geom pos="0.513 0 0.01497" material="wide_angle_camera" mesh="wide_angle_camera"
        class="visual" />
      <geom pos="-0.4145 0 0" material="face" mesh="face" class="visual_zflip" />
      <geom class="collision" size="0.055 0.07 0.09" pos="-0.4695 0 0" quat="0 0 0 1" type="box" />
      <geom class="collision" size="0.09 0.055" pos="-0.4695 -0.07 0" quat="0 -1 0 1" />
      <geom class="collision" size="0.09 0.055" pos="-0.4695 0.07 0" quat="0 -1 0 1" />
      <geom pos="-0.46165 0 -0.0292" quat="0 -0.258819 0 0.965926" material="depth_camera"
        mesh="depth_camera"
        class="visual" />
      <geom pos="-0.513 0 0.01497" material="wide_angle_camera" mesh="wide_angle_camera"
        class="visual_zflip" />
      <geom material="battery" mesh="battery" class="visual" />
      <geom pos="0 0.07646 0.02905" quat="0.683013 -0.183013 0.183013 0.683013"
        material="depth_camera"
        mesh="depth_camera" class="visual" />
      <geom pos="0 -0.07646 0.02905" quat="0.683013 0.183013 0.183013 -0.683013"
        material="depth_camera"
        mesh="depth_camera" class="visual" />
      <geom pos="-0.364 0 0.0735" material="lidar_cage" mesh="lidar_cage" class="visual" />
      <geom class="collision" size="0.07 0.06" pos="-0.364 0 0.1335" />
      <geom pos="-0.364 0 0.1422" quat="1 0 0 -1" material="lidar" mesh="lidar" class="visual" />
      <geom pos="0.2999 0.104 0" quat="0.258819 0.965926 0 0" material="drive" mesh="drive"
        class="visual" />
      <geom pos="0.2999 -0.104 0" quat="0.258819 -0.965926 0 0" material="drive" mesh="drive"
        class="visual" />
      <geom pos="-0.2999 0.104 0" quat="0 0 0.965926 -0.258819" material="drive" mesh="drive"
        class="visual" />
      <geom pos="-0.2999 -0.104 0" quat="0 0 -0.965926 -0.258819" material="drive" mesh="drive"
        class="visual" />
      <geom pos="0.116 0 0.073" material="hatch" mesh="hatch" class="visual" />
      <body name="LF_HIP" pos="0.2999 0.104 0" quat="0.258819 0.965926 0 0">
        <inertial mass="2.781" pos="0.0566606 -0.015294 -0.00829784"
          quat="-0.127978 0.709783 -0.135278 0.679359"
          diaginertia="0.00585729 0.00491868 0.00329081" />
        <joint name="LF_HAA" axis="1 0 0" range="-0.72 0.49" />
        <geom quat="0.258819 -0.965926 0 0" material="hip_l" mesh="hip_l" class="visual" />
        <geom pos="0.0599 -0.0725816 -0.041905" quat="0.183013 -0.683013 0.683013 0.183013"
          material="drive"
          mesh="drive" class="visual" />
        <geom class="collision" size="0.05 0.07" pos="0.0599 -0.0119598 -0.006905"
          quat="-0.353553 -0.612372 0.612372 -0.353553" />
        <body name="LF_THIGH" pos="0.0599 -0.0725816 -0.041905"
          quat="0.183013 -0.683013 0.683013 0.183013">
          <inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
            quat="0.993166 -0.00515309 -0.0806592 0.0841972"
            diaginertia="0.03025 0.0298943 0.00418465" />
          <joint name="LF_HFE" axis="1 0 0" range="-9.42478 9.42478" />
          <geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
          <geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 -0.5 -0.5" />
          <geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 -1"
            type="box" />
          <geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
          <geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
          <body name="LF_SHANK" pos="0.1003 0 -0.285">
            <inertial mass="0.58842" pos="0.005462 -0.0612528 -0.0806598"
              quat="0.992934 -0.115904 -0.00105487 -0.0254421"
              diaginertia="0.0101637 0.00923838 0.00111927" />
            <joint name="LF_KFE" axis="1 0 0" range="-9.42478 9.42478" />
            <geom quat="1 0 0 -1" material="shank_l" mesh="shank_l" class="visual" />
            <geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 -0.5 -0.5" />
            <geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 -0.057499 0"
              quat="0.5 0.5 -0.5 -0.5"
              type="box" />
            <geom pos="0.01305 -0.08795 -0.33797" quat="0.382683 0 0 -0.92388" material="foot"
              mesh="foot"
              class="visual" />
            <geom class="collision" size="0.0175 0.141252" pos="0.01305 -0.08795 -0.168985"
              quat="1 0 0 -1" />
            <geom name="LF_FOOT" class="foot" pos="0.01305 -0.08795 -0.31547" quat="1 0 0 -1" />
          </body>
        </body>
      </body>
      <body name="RF_HIP" pos="0.2999 -0.104 0" quat="0.258819 -0.965926 0 0">
        <inertial mass="2.781" pos="0.0567633 0.015294 -0.00829784"
          quat="0.13524 0.679072 0.127985 0.710065"
          diaginertia="0.00585928 0.0049205 0.00329064" />
        <joint name="RF_HAA" axis="1 0 0" range="-0.49 0.72" />
        <geom quat="0.258819 0.965926 0 0" material="hip_r" mesh="hip_r" class="visual" />
        <geom pos="0.0599 0.0725816 -0.041905" quat="0.183013 0.683013 0.683013 -0.183013"
          material="drive" mesh="drive"
          class="visual" />
        <geom class="collision" size="0.05 0.07" pos="0.0599 0.0119598 -0.006905"
          quat="-0.353553 0.612372 0.612372 0.353553" />
        <body name="RF_THIGH" pos="0.0599 0.0725816 -0.041905"
          quat="0.183013 0.683013 0.683013 -0.183013">
          <inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
            quat="0.992775 -0.00512735 -0.0806685 0.0886811"
            diaginertia="0.0302511 0.0298933 0.0041845" />
          <joint name="RF_HFE" axis="-1 0 0" range="-9.42478 9.42478" />
          <geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
          <geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 0.5 0.5" />
          <geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 1"
            type="box" />
          <geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
          <geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
          <body name="RF_SHANK" pos="0.1003 0 -0.285">
            <inertial mass="0.58842" pos="0.005462 0.0612528 -0.0806598"
              quat="0.992934 0.115904 -0.00105487 0.0254421"
              diaginertia="0.0101637 0.00923838 0.00111927" />
            <joint name="RF_KFE" axis="-1 0 0" range="-9.42478 9.42478" />
            <geom quat="1 0 0 1" material="shank" mesh="shank_r" class="visual" />
            <geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 0.5 0.5" />
            <geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 0.057499 0"
              quat="0.5 0.5 0.5 0.5"
              type="box" />
            <geom pos="0.01305 0.08795 -0.33797" quat="0.382683 0 0 0.92388" material="foot"
              mesh="foot" class="visual" />
            <geom class="collision" size="0.0175 0.141252" pos="0.01305 0.08795 -0.168985"
              quat="1 0 0 1" />
            <geom name="RF_FOOT" class="foot" pos="0.01305 0.08795 -0.31547" quat="1 0 0 1" />
          </body>
        </body>
      </body>
      <body name="LH_HIP" pos="-0.2999 0.104 0" quat="0 0 0.965926 -0.258819">
        <inertial mass="2.781" pos="0.0567633 0.015294 -0.00829784"
          quat="0.13524 0.679072 0.127985 0.710065"
          diaginertia="0.00585928 0.0049205 0.00329064" />
        <joint name="LH_HAA" axis="-1 0 0" range="-0.72 0.49" />
        <geom quat="-0.258819 -0.965926 0 0" material="hip_r" mesh="hip_r" class="visual" />
        <geom pos="0.0599 0.0725816 -0.041905" quat="0.183013 0.683013 0.683013 -0.183013"
          material="drive" mesh="drive"
          class="visual" />
        <geom class="collision" size="0.05 0.07" pos="0.0599 0.0119598 -0.006905"
          quat="-0.353553 0.612372 0.612372 0.353553" />
        <body name="LH_THIGH" pos="0.0599 0.0725816 -0.041905"
          quat="0.183013 0.683013 0.683013 -0.183013">
          <inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
            quat="0.992775 -0.00512735 -0.0806685 0.0886811"
            diaginertia="0.0302511 0.0298933 0.0041845" />
          <joint name="LH_HFE" axis="1 0 0" range="-9.42478 9.42478" />
          <geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
          <geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 -0.5 -0.5" />
          <geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 -1"
            type="box" />
          <geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
          <geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
          <body name="LH_SHANK" pos="0.1003 0 -0.285">
            <inertial mass="0.58842" pos="0.005462 0.0612528 -0.0806598"
              quat="0.992934 0.115904 -0.00105487 0.0254421"
              diaginertia="0.0101637 0.00923838 0.00111927" />
            <joint name="LH_KFE" axis="1 0 0" range="-9.42478 9.42478" />
            <geom quat="-1 0 0 -1" material="shank" mesh="shank_r" class="visual" />
            <geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 -0.5 -0.5" />
            <geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 0.057499 0"
              quat="0.5 0.5 -0.5 -0.5"
              type="box" />
            <geom pos="0.01305 0.08795 -0.33797" quat="-0.382683 0 0 -0.92388" material="foot"
              mesh="foot"
              class="visual" />
            <geom class="collision" size="0.0175 0.141252" pos="0.01305 0.08795 -0.168985"
              quat="1 0 0 -1" />
            <geom name="LH_FOOT" class="foot" pos="0.01305 0.08795 -0.31547" quat="1 0 0 -1" />
          </body>
        </body>
      </body>
      <body name="RH_HIP" pos="-0.2999 -0.104 0" quat="0 0 -0.965926 -0.258819">
        <inertial mass="2.781" pos="0.0566606 -0.015294 -0.00829784"
          quat="-0.127978 0.709783 -0.135278 0.679359"
          diaginertia="0.00585729 0.00491868 0.00329081" />
        <joint name="RH_HAA" axis="-1 0 0" range="-0.49 0.72" />
        <geom quat="-0.258819 0.965926 0 0" material="hip_l" mesh="hip_l" class="visual" />
        <geom pos="0.0599 -0.0725816 -0.041905" quat="-0.183013 0.683013 -0.683013 -0.183013"
          material="drive"
          mesh="drive" class="visual" />
        <geom class="collision" size="0.05 0.07" pos="0.0599 -0.0119598 -0.006905"
          quat="0.353553 0.612372 -0.612372 0.353553" />
        <body name="RH_THIGH" pos="0.0599 -0.0725816 -0.041905"
          quat="-0.183013 0.683013 -0.683013 -0.183013">
          <inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
            quat="0.993166 -0.00515309 -0.0806592 0.0841972"
            diaginertia="0.03025 0.0298943 0.00418465" />
          <joint name="RH_HFE" axis="-1 0 0" range="-9.42478 9.42478" />
          <geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
          <geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 0.5 0.5" />
          <geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 1"
            type="box" />
          <geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
          <geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
          <body name="RH_SHANK" pos="0.1003 0 -0.285">
            <inertial mass="0.58842" pos="0.005462 -0.0612528 -0.0806598"
              quat="0.992934 -0.115904 -0.00105487 -0.0254421"
              diaginertia="0.0101637 0.00923838 0.00111927" />
            <joint name="RH_KFE" axis="-1 0 0" range="-9.42478 9.42478" />
            <geom quat="1 0 0 -1" material="shank_l" mesh="shank_l" class="visual" />
            <geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 0.5 0.5" />
            <geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 -0.057499 0"
              quat="0.5 0.5 0.5 0.5"
              type="box" />
            <geom pos="0.01305 -0.08795 -0.33797" quat="0.382683 0 0 -0.92388" material="foot"
              mesh="foot"
              class="visual" />
            <geom class="collision" size="0.0175 0.141252" pos="0.01305 -0.08795 -0.168985"
              quat="1 0 0 1" />
            <geom name="RH_FOOT" class="foot" pos="0.01305 -0.08795 -0.31547" quat="1 0 0 1" />
          </body>
        </body>
      </body>
    </body>

    <!-- Current heading arrow (green) - use freejoint to control position, no physical effect -->
    <body name="robot_heading_arrow" pos="0 0 0.76" mocap="true">
      <geom name="robot_arrow_head" type="mesh" mesh="arrow"
        pos="0.25 0.0 0.08" quat="1 0 0 0"
        rgba="0 1 0 0.8" contype="0" conaffinity="0" />
    </body>

    <!-- Desired heading arrow (blue) - use freejoint to control position, no physical effect -->
    <body name="desired_heading_arrow" pos="0 0 0.76" mocap="true">
      <geom name="desired_arrow_head" type="mesh" mesh="arrow"
        pos="0.25 0.0 0.08" quat="1 0 0 0"
        rgba="0 0 1 0.8" contype="0" conaffinity="0" />
    </body>
  </worldbody>

  <contact>
    <exclude body1="base" body2="LF_THIGH" />
    <exclude body1="base" body2="RF_THIGH" />
    <exclude body1="base" body2="LH_THIGH" />
    <exclude body1="base" body2="RH_THIGH" />
  </contact>

  <sensor>
    <framelinvel name="base_linvel" objtype="body" objname="base" />
    <gyro name="base_gyro" site="imu_site" />
  </sensor>

  <actuator>
    <position class="affine" joint="LF_HAA" name="LF_HAA" />
    <position class="affine" joint="LF_HFE" name="LF_HFE" />
    <position class="affine" joint="LF_KFE" name="LF_KFE" />
    <position class="affine" joint="RF_HAA" name="RF_HAA" />
    <position class="affine" joint="RF_HFE" name="RF_HFE" />
    <position class="affine" joint="RF_KFE" name="RF_KFE" />
    <position class="affine" joint="LH_HAA" name="LH_HAA" />
    <position class="affine" joint="LH_HFE" name="LH_HFE" />
    <position class="affine" joint="LH_KFE" name="LH_KFE" />
    <position class="affine" joint="RH_HAA" name="RH_HAA" />
    <position class="affine" joint="RH_HFE" name="RH_HFE" />
    <position class="affine" joint="RH_KFE" name="RH_KFE" />
  </actuator>
</mujoco>