<mujoco>
  <compiler meshdir="cars/meshes" />
  <asset>
    <mesh name="goose_mushr_base_nano" file="mushr_base_nano.stl"/>
    <mesh name="goose_mushr_wheel" file="mushr_wheel.stl"/>
    <mesh name="goose_mushr_ydlidar" file="mushr_ydlidar.stl"/>
  </asset>
  <default>
      <default class="goose_wheel">
      <geom fitscale="1.2" type="ellipsoid" friction="2 0.005 0.0001" contype="1" conaffinity="0" mesh="goose_mushr_wheel" mass="0.498952"/>
    </default>
    <default class="goose_steering">
      <joint type="hinge" axis="0 0 1" limited="true" frictionloss="0.01" damping="0.001" armature="0.0002" range="-0.38 0.38"/>
    </default>
    <default class="goose_throttle">
      <joint type="hinge" axis="0 1 0" frictionloss="0.001" damping="0.01" armature="0.01" limited="false"/>
    </default>
  </default>
  <worldbody>
    <body name="goose" pos="0.0 3.0 0.0" euler="0 0 0.0">
      <camera name="goose_third_person" mode="fixed" pos="-1 0 1" xyaxes="0 -1 0 0.707 0 0.707"/>
      <joint type="free"/>

      <camera name="goose_realsense_d435i" mode="fixed" pos="-0.005 0 .165" euler="0 4.712 4.712"/>
      <site name="goose_imu" pos="-0.005 0 .165"/>

      <geom pos="0 0 0.094655" type="mesh" mass="3.542137" mesh="goose_mushr_base_nano"/>
      <geom name="goose_realsense_d435i" size="0.012525 0.045 0.0125" pos="0.0123949 0 0.162178" mass="0.072" type="box"/>
      <geom name="goose_ydlidar" pos="-0.035325 0 0.202405" type="mesh" mass="0.180" mesh="goose_mushr_ydlidar"/>

      <body name="goose_steering_wheel" pos="0.1385 0 0.0488">
        <joint class="goose_steering" name="goose_steering_wheel"/>
        <geom class="goose_wheel" contype="0" conaffinity="0" mass="0.01" rgba="0 0 0 0.01"/>
      </body>

      <body name="goose_wheel_fl" pos="0.1385 0.115 0.0488">
        <joint class="goose_steering" name="goose_wheel_fl_steering"/>
        <joint class="goose_throttle" name="goose_wheel_fl_throttle"/>
        <geom class="goose_wheel"/>
        <geom class="goose_wheel" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 0.3"/>
      </body>
      <body name="goose_wheel_fr" pos="0.1385 -0.115 0.0488">
        <joint class="goose_steering" name="goose_wheel_fr_steering"/>
        <joint class="goose_throttle" name="goose_wheel_fr_throttle"/>
        <geom class="goose_wheel"/>
        <geom class="goose_wheel" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 0.3"/>
      </body>
      <body name="goose_wheel_bl" pos="-0.158 0.115 0.0488">
        <joint class="goose_throttle" name="goose_wheel_bl_throttle"/>
        <geom class="goose_wheel"/>
        <geom class="goose_wheel" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 0.3"/>
      </body>
      <body name="goose_wheel_br" pos="-0.158 -0.115 0.0488">
        <joint class="goose_throttle" name="goose_wheel_br_throttle"/>
        <geom class="goose_wheel"/>
        <geom class="goose_wheel" type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 0.3"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <position class="goose_steering" kp="25.0" name="goose_steering_pos" joint="goose_steering_wheel"/>
    <velocity kv="100" gear="0.04" forcelimited="true" forcerange="-500 500" name="goose_throttle_velocity" tendon="goose_throttle"/>
  </actuator>
  <equality>
    <!-- taylor expansion of delta_l = arctan(L/(L/tan(delta) - W/2)) with L,W in reference to kinematic car model -->
    <joint joint1="goose_wheel_fl_steering" joint2="goose_steering_wheel" polycoef="0 1 0.375 0.140625 -0.0722656"/>

    <!-- taylor expansion of delta_r = arctan(L/(L/tan(delta) + W/2)) with L,W in reference to kinematic car model -->
    <joint joint1="goose_wheel_fr_steering" joint2="goose_steering_wheel" polycoef="0 1 -0.375 0.140625 0.0722656"/>
  </equality>
  <tendon>
    <fixed name="goose_throttle">
      <joint joint="goose_wheel_fl_throttle" coef="0.25"/>
      <joint joint="goose_wheel_fr_throttle" coef="0.25"/>
      <joint joint="goose_wheel_bl_throttle" coef="0.25"/>
      <joint joint="goose_wheel_br_throttle" coef="0.25"/>
    </fixed>
  </tendon>
  <sensor>
    <accelerometer name="goose_accelerometer" site="goose_imu" />
    <gyro name="goose_gyro" site="goose_imu" />
    <velocimeter name="goose_velocimeter" site="goose_imu" />
  </sensor>
</mujoco>
