diff --git a/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch b/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch
index 309a20155..f1610995b 100644
--- a/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch
+++ b/bitbots_misc/bitbots_bringup/launch/simulator_teamplayer.launch
@@ -10,10 +10,15 @@
     <arg name="teamcom" default="false" description="Whether the team communication system should be started" />
     <arg name="vision" default="true" description="Whether the vision system should be started" />
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
+    <arg name="mujoco" default="false" description="Whether to use the Mujoco simulator instead of Webots"/>
 
     <!-- load the general simulator -->
-    <include file="$(find-pkg-share bitbots_webots_sim)/launch/simulation.launch" />
-
+    <group if="$(var mujoco)">
+        <node pkg="bitbots_mujoco_sim" exec="sim" name="mujoco_simulation_node" output="screen"/>
+    </group>
+    <group unless="$(var mujoco)">
+        <include file="$(find-pkg-share bitbots_webots_sim)/launch/simulation.launch" />
+    </group>
     <!-- load teamplayer software stack -->
     <include file="$(find-pkg-share bitbots_bringup)/launch/teamplayer.launch">
         <arg name="behavior" value="$(var behavior)" />
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Camera.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Camera.py
new file mode 100644
index 000000000..26bdc5ebe
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Camera.py
@@ -0,0 +1,59 @@
+import mujoco
+import numpy as np
+
+
+class Camera:
+    """Represents a camera in the MuJoCo simulation, providing image rendering capabilities."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData, name: str, width: int = 800, height: int = 600):
+        self.model: mujoco.MjModel = model
+        self.data: mujoco.MjData = data
+        self.name: str = name
+        self.instance = model.camera(name)
+        self.width: int = width
+        self.height: int = height
+        self.renderer = mujoco.Renderer(self.model, height=self.height, width=self.width)
+
+    @property
+    def fov(self) -> float:
+        """Returns the camera's horizontal field of view in radians."""
+        if (hasattr(self, "_fov") and self._fov is not None):
+            return self._fov
+
+        # MuJoCo uses fovy (vertical field of view in degrees)
+        fovy_deg = (
+            self.instance.fovy[0]
+            if hasattr(self.instance.fovy, "__iter__")
+            else self.instance.fovy
+        )
+        fovy_rad = np.deg2rad(fovy_deg)
+
+        # Convert vertical FOV to horizontal FOV using aspect ratio
+        aspect_ratio = self.width / self.height
+        fovx_rad = 2 * np.arctan(np.tan(fovy_rad / 2) * aspect_ratio)
+
+        self._fov = fovx_rad
+        return self._fov
+
+    def render(self) -> bytes:
+        """
+        Renders and returns the camera image as raw bytes (BGRA format).
+        Returns Raw image data in BGRA8 format for ROS Image messages.
+        """
+        # Update renderer with current scene
+        self.renderer.update_scene(self.data, camera=self.name)
+
+        # Render the image - returns RGB by default
+        rgb_array = self.renderer.render()
+
+        # Convert RGB to BGRA (standard for ROS Image messages)
+        # MuJoCo returns RGB uint8 array of shape (height, width, 3)
+        # We need to add alpha channel and swap R and B
+        height, width, _ = rgb_array.shape
+        bgra_array = np.zeros((height, width, 4), dtype=np.uint8)
+        bgra_array[:, :, 0] = rgb_array[:, :, 2]  # B
+        bgra_array[:, :, 1] = rgb_array[:, :, 1]  # G
+        bgra_array[:, :, 2] = rgb_array[:, :, 0]  # R
+        bgra_array[:, :, 3] = 255  # A (fully opaque)
+
+        return bgra_array.tobytes()
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Joint.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Joint.py
new file mode 100644
index 000000000..04dfb05bb
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Joint.py
@@ -0,0 +1,39 @@
+import mujoco
+
+
+class Joint:
+    """Represents a single controllable joint and associated actuator in the MuJoCo simulation."""
+
+    def __init__(
+        self,
+        model: mujoco.MjModel,
+        data: mujoco.MjData,
+        ros_name: str,
+        name: str | None = None,
+    ):
+        self.model: mujoco.MjModel = model
+        self.data: mujoco.MjData = data
+        self.ros_name: str = ros_name
+        self.name: str = name if name is not None else ros_name
+        self.joint_instance: int = model.joint(self.name)
+        self.actuator_instance: int = model.actuator(self.name)
+
+    @property
+    def position(self) -> float:
+        """Gets the current joint position (angle) in radians."""
+        return self.data.qpos[self.joint_instance.qposadr[0]]
+    
+    @position.setter
+    def position(self, value: float) -> None:
+        """Sets the position target for the joint's position actuator."""
+        self.data.ctrl[self.actuator_instance.id] = value
+
+    @property
+    def velocity(self) -> float:
+        """Gets the current joint velocity in rad/s."""
+        return self.data.qvel[self.joint_instance.dofadr[0]]
+
+    @property
+    def effort(self) -> float:
+        """Gets the current effort (force/torque) applied by the position actuator."""
+        return self.data.actuator_force[self.actuator_instance.id]
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Robot.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Robot.py
new file mode 100644
index 000000000..c3e38c699
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Robot.py
@@ -0,0 +1,78 @@
+import mujoco
+
+from bitbots_mujoco_sim.Camera import Camera
+from bitbots_mujoco_sim.Joint import Joint
+from bitbots_mujoco_sim.Sensor import Sensor
+
+
+class Robot:
+    """Represents the robot, holding all its components like the camera, joints, and sensors."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData):
+        self.camera = Camera(model, data, name="head_camera")
+        self.joints = RobotJoints(
+            [
+                # --- Right Leg ---
+                Joint(model, data, ros_name="RHipYaw"),
+                Joint(model, data, ros_name="RHipRoll"),
+                Joint(model, data, ros_name="RHipPitch"),
+                Joint(model, data, ros_name="RKnee"),
+                Joint(model, data, ros_name="RAnklePitch"),
+                Joint(model, data, ros_name="RAnkleRoll"),
+                # --- Left Leg ---
+                Joint(model, data, ros_name="LHipYaw"),
+                Joint(model, data, ros_name="LHipRoll"),
+                Joint(model, data, ros_name="LHipPitch"),
+                Joint(model, data, ros_name="LKnee"),
+                Joint(model, data, ros_name="LAnklePitch"),
+                Joint(model, data, ros_name="LAnkleRoll"),
+                # --- Arms ---
+                Joint(model, data, ros_name="RShoulderPitch"),
+                Joint(model, data, ros_name="RShoulderRoll"),
+                Joint(model, data, ros_name="RElbow"),
+                Joint(model, data, ros_name="LShoulderPitch"),
+                Joint(model, data, ros_name="LShoulderRoll"),
+                Joint(model, data, ros_name="LElbow"),
+                # --- Head ---
+                Joint(model, data, ros_name="HeadPan"),
+                Joint(model, data, ros_name="HeadTilt"),
+            ]
+        )
+        self.sensors = RobotSensors(
+            [
+                # IMU Sensors
+                Sensor(model, data, name="gyro", ros_name="IMU_gyro"),
+                Sensor(model, data, name="accelerometer", ros_name="IMU_accelerometer"),
+                Sensor(model, data, name="orientation", ros_name="IMU_orientation",),  # Global orientation quaternion
+                Sensor(model, data, name="position", ros_name="IMU_position"),  # Global position vector
+                # Foot Sensors
+                Sensor(model, data, name="l_foot_pos", ros_name="left_foot_position"),
+                Sensor(model, data, name="r_foot_pos", ros_name="right_foot_position"),
+                Sensor(model, data, name="l_foot_global_linvel", ros_name="left_foot_velocity"),
+                Sensor(model, data, name="r_foot_global_linvel", ros_name="right_foot_velocity"),
+            ]
+        )
+
+
+class RobotSensors(list[Sensor]):
+    """A list of Robot Sensors with additional helper methods."""
+
+    def get(self, name: str) -> Joint:
+        """Finds and returns a specific joint by its ROS name."""
+        return next(filter(lambda joint: joint.ros_name == name, self))
+
+    @property
+    def gyro(self) -> Sensor:
+        return self.get("IMU_gyro")
+
+    @property
+    def accelerometer(self) -> Sensor:
+        return self.get("IMU_accelerometer")
+
+
+class RobotJoints(list[Joint]):
+    """A list of Robot Joints with additional helper methods."""
+
+    def get(self, name: str) -> Joint:
+        """Finds and returns a specific joint by its ROS name."""
+        return next(filter(lambda joint: joint.ros_name == name, self))
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Sensor.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Sensor.py
new file mode 100644
index 000000000..e787e541f
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Sensor.py
@@ -0,0 +1,28 @@
+import mujoco
+import numpy as np
+
+
+class Sensor:
+    """Represents a single sensor, providing a clean interface to its value."""
+
+    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData, name: str, ros_name: str):
+        self._data = data
+        self.ros_name: str = ros_name
+        self.instance: int = model.sensor(name)
+        self.id: int = self.instance.id
+
+    @property
+    def data(self) -> np.ndarray:
+        """Gets the current sensor reading from sensordata array."""
+        return self._data.sensordata[self.adr : self.adr + self.dim]
+
+    @property
+    def adr(self) -> int:
+        """Gets the address of the sensor data in sensordata array."""
+        return int(self.instance.adr)
+
+    @property
+    def dim(self) -> int:
+        """Gets the dimension of the sensor data."""
+        return int(self.instance.dim)
+
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Simulation.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Simulation.py
new file mode 100644
index 000000000..e9dae7040
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/Simulation.py
@@ -0,0 +1,160 @@
+import time
+import math
+
+import mujoco
+from ament_index_python.packages import get_package_share_directory
+from mujoco import viewer
+from rclpy.node import Node
+from rclpy.time import Time
+from rosgraph_msgs.msg import Clock
+from sensor_msgs.msg import CameraInfo, Imu, JointState, Image
+
+from bitbots_msgs.msg import JointCommand
+from bitbots_mujoco_sim.Robot import Robot
+
+
+class Simulation(Node):
+    """Manages the MuJoCo simulation state and its components."""
+
+    def __init__(self):
+        super().__init__("sim_interface")
+        package_path = get_package_share_directory("bitbots_mujoco_sim")
+        self.model: mujoco.MjModel = mujoco.MjModel.from_xml_path(package_path + "/xml/adult_field.xml")
+        self.data: mujoco.MjData = mujoco.MjData(self.model)
+        self.robot: Robot = Robot(self.model, self.data)
+
+        self.time = 0.0
+        self.time_message = Time(seconds=0, nanoseconds=0).to_msg()
+        self.timestep = self.model.opt.timestep
+        self.step_number = 0
+        self.clock_publisher = self.create_publisher(Clock, "clock", 1)
+        
+        self.js_publisher = self.create_publisher(JointState, "joint_states", 1)
+        self.create_subscription(JointCommand, "DynamixelController/command", self.joint_command_callback, 1)
+        
+        self.imu_frame_id = self.get_parameter_or("imu_frame", "imu_frame")
+        self.imu_publisher = self.create_publisher(Imu, "imu/data_raw", 1)
+        
+        self.camera_active = True
+        self.camera_optical_frame_id = self.get_parameter_or("camera_optical_frame", "camera_optical_frame")
+        self.camera_publisher = self.create_publisher(Image, "camera/image_proc", 1)
+        self.camera_info_publisher = self.create_publisher(CameraInfo, "camera/camera_info", 1) 
+
+        self.events = {
+            "clock": {"frequency": 1, "handler": self.publish_clock_event},
+            "joint_states": {"frequency": 3, "handler": self.publish_ros_joint_states_event},
+            "imu": {"frequency": 3, "handler": self.publish_imu_event},
+            "camera": {"frequency": 24, "handler": self.publish_camera_event},
+        }
+
+    def run(self) -> None:
+        print("Starting simulation viewer...")
+        with viewer.launch_passive(self.model, self.data) as view:
+            while view.is_running():
+                self.step()
+                view.sync()
+
+    def joint_command_callback(self, command: JointCommand) -> None:
+        if len(command.positions) != 0:
+            for i in range(len(command.joint_names)):
+                joint = self.robot.joints.get(command.joint_names[i])
+                joint.position = command.positions[i]
+                #if len(command.velocities) != 0:
+                #    joint.velocity = command.velocities[i]
+
+    def step(self) -> None:
+        real_start_time = time.time()
+        self.step_number += 1
+        self.time += self.timestep
+        self.time_message = Time(seconds=int(self.time), nanoseconds=int(self.time % 1 * 1e9)).to_msg()
+
+        mujoco.mj_step(self.model, self.data)
+
+        for event_name, event_config in self.events.items():
+            if self.step_number % event_config["frequency"] == 0:
+                event_config["handler"]()
+
+        real_end_time = time.time()
+        time.sleep(max(0.0, self.timestep - (real_end_time - real_start_time)))
+
+    def publish_clock_event(self) -> None:
+        clock_msg = Clock()
+        clock_msg.clock = self.time_message
+        self.clock_publisher.publish(clock_msg)
+
+    def publish_ros_joint_states_event(self) -> None:
+        js = JointState()
+        js.name = []
+        js.header.stamp = self.time_message
+        js.position = []
+        js.effort = []
+        for joint in self.robot.joints:
+            js.name.append(joint.ros_name)
+            js.position.append(joint.position)
+            js.velocity.append(joint.velocity)
+            js.effort.append(joint.effort)
+        self.js_publisher.publish(js)
+
+    def publish_imu_event(self) -> None:
+        imu = Imu()
+        imu.header.stamp = self.time_message
+        imu.header.frame_id = self.imu_frame_id
+        imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z = self.robot.sensors.accelerometer.data
+
+        # make sure that acceleration is not completely zero or we will get error in filter.
+        # Happens if robot is moved manually in the simulation
+        if imu.linear_acceleration.x == 0 and imu.linear_acceleration.y == 0 and imu.linear_acceleration.z == 0:
+            imu.linear_acceleration.z = 0.001
+
+        imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z = self.robot.sensors.gyro.data
+
+        self.imu_publisher.publish(imu)
+
+    def publish_camera_event(self) -> None:
+        if not self.camera_active:
+            return
+        
+        img = Image()
+        img.header.stamp = self.time_message
+        img.header.frame_id = self.camera_optical_frame_id
+        img.encoding = "bgra8"
+        img.height = self.robot.camera.height
+        img.width = self.robot.camera.width
+        img.step = 4 * self.robot.camera.width
+        img.data = self.robot.camera.render()
+        self.camera_publisher.publish(img)
+
+        cam_info = CameraInfo()
+        cam_info.header = img.header
+        cam_info.height = self.robot.camera.height
+        cam_info.width = self.robot.camera.width
+
+        @staticmethod
+        def focal_length_from_fov(fov: float, resolution: int) -> float:
+            "Calculate focal length from field of view and resolution."
+            return 0.5 * resolution * (math.cos(fov / 2) / math.sin(fov / 2))
+
+        @staticmethod
+        def h_fov_to_v_fov(h_fov: float, height: int, width: int) -> float:
+            "Convert horizontal FOV to vertical FOV based on image aspect ratio."
+            return 2 * math.atan(math.tan(h_fov * 0.5) * (height / width))
+        
+        camera = self.robot.camera
+
+        h_fov = camera.fov
+        v_fov = h_fov_to_v_fov(h_fov, camera.height, camera.width)
+        
+        f_x, f_y = focal_length_from_fov(h_fov, camera.width), focal_length_from_fov(v_fov, camera.height)
+        cx, cy = camera.width / 2.0, camera.height / 2.0
+        cam_info.k = [
+            f_x, 0.0, cx,
+            0.0, f_y, cy,
+            0.0, 0.0, 1.0
+        ]
+        cam_info.p = [
+            f_x, 0.0, cx, 0.0,
+            0.0, f_y, cy, 0.0,
+            0.0, 0.0, 1.0, 0.0
+        ]
+        
+        self.camera_info_publisher.publish(cam_info)
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/__init__.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/__init__.py
new file mode 100644
index 000000000..2cdb94c9a
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/__init__.py
@@ -0,0 +1,7 @@
+"""BitBots MuJoCo Simulation Package"""
+
+from .Simulation import Simulation
+
+__all__ = [
+    "Simulation",
+]
diff --git a/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/main.py b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/main.py
new file mode 100644
index 000000000..89e3ec586
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/bitbots_mujoco_sim/main.py
@@ -0,0 +1,18 @@
+import math
+
+import rclpy
+
+from bitbots_mujoco_sim.Simulation import Simulation
+import threading
+from rclpy.experimental.events_executor import EventsExecutor
+
+def main(args=None):
+    rclpy.init(args=args)
+    simulation = Simulation()
+    executor = EventsExecutor()
+    executor.add_node(simulation)
+    thread = threading.Thread(target=executor.spin, daemon=True)
+    thread.start()
+    simulation.run()
+    simulation.destroy_node()
+    rclpy.shutdown()
diff --git a/bitbots_simulation/bitbots_mujoco_sim/package.xml b/bitbots_simulation/bitbots_mujoco_sim/package.xml
new file mode 100644
index 000000000..ca5137d72
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>bitbots_mujoco_sim</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="22maschal@todo.todo">22maschal</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/bitbots_simulation/bitbots_mujoco_sim/resource/bitbots_mujoco_sim b/bitbots_simulation/bitbots_mujoco_sim/resource/bitbots_mujoco_sim
new file mode 100644
index 000000000..e69de29bb
diff --git a/bitbots_simulation/bitbots_mujoco_sim/setup.cfg b/bitbots_simulation/bitbots_mujoco_sim/setup.cfg
new file mode 100644
index 000000000..de24bec2b
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/bitbots_mujoco_sim
+[install]
+install_scripts=$base/lib/bitbots_mujoco_sim
diff --git a/bitbots_simulation/bitbots_mujoco_sim/setup.py b/bitbots_simulation/bitbots_mujoco_sim/setup.py
new file mode 100644
index 000000000..1ce730972
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/setup.py
@@ -0,0 +1,34 @@
+import glob
+
+from setuptools import find_packages, setup
+
+package_name = "bitbots_mujoco_sim"
+
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/xml", glob.glob("xml/*.xml")),
+        ("share/" + package_name + "/xml/assets", glob.glob("xml/assets/*.png")),
+        ("share/" + package_name + "/xml/assets", glob.glob("xml/assets/*.stl")),
+        ("share/" + package_name + "/xml/assets/ball", glob.glob("xml/assets/ball/*.png")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="22maschal",
+    maintainer_email="22maschal@todo.todo",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    extras_require={
+        "test": [
+            "pytest",
+        ],
+    },
+    entry_points={
+        "console_scripts": ["sim = bitbots_mujoco_sim.main:main"],
+    },
+)
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml b/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml
new file mode 100644
index 000000000..71d69326a
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/xml/adult_field.xml
@@ -0,0 +1,29 @@
+<mujoco>
+  <compiler angle="radian" assetdir="assets"/>
+  <option timestep="0.002" density="1.2" viscosity="0.00002"/>
+  
+  <visual>
+    <global offwidth="800" offheight="600"/>
+  </visual>
+  
+  <include file="goal.xml"/>
+  <include file="wolfgang.xml"/>
+
+  <asset>
+    <texture name="pitch" file="adult_field.png" type="2d"/>
+    <material name="pitch" reflectance="0.0" texture="pitch" texrepeat="1 1" texuniform="false"/>
+    <texture name="ball" type="cube" fileup="ball/up.png" filedown="ball/down.png" filefront="ball/front.png" fileback="ball/back.png" fileleft="ball/left.png" fileright="ball/right.png"/>
+    <material name="ball" texture="ball"/>
+  </asset>
+
+  <worldbody>
+    <light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
+    <geom material="pitch" name="pitch" pos="0 0 0" size="8 5.5 1" type="plane"/>
+
+    <body name="ball" pos="0 0 0.11">
+      <freejoint name="ball-root"/>
+      <site name='B-vismarker' pos='0 0 0'/>
+      <geom name="ball" pos="3 0 0" size="0.11" mass="0.41" friction="0.4 0.01 0.01" rgba="1 1 1 1" condim="6" priority="1" solref="-5000 -20" type="sphere" material="ball"/>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field.png
new file mode 100644
index 000000000..c11ffe335
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field_original.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field_original.png
new file mode 100644
index 000000000..835fa2c48
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/adult_field_original.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ankle.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ankle.stl
new file mode 100644
index 000000000..7ea29289a
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ankle.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/back.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/back.png
new file mode 100644
index 000000000..c01b51714
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/back.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/down.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/down.png
new file mode 100644
index 000000000..49ace5b65
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/down.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/front.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/front.png
new file mode 100644
index 000000000..c18fbf046
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/front.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/left.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/left.png
new file mode 100644
index 000000000..d3b14f7b7
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/left.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/right.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/right.png
new file mode 100644
index 000000000..69965a816
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/right.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/up.png b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/up.png
new file mode 100644
index 000000000..8c729d705
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/ball/up.png differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/baseplate_odroid_xu4_core.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/baseplate_odroid_xu4_core.stl
new file mode 100644
index 000000000..4660be2d7
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/baseplate_odroid_xu4_core.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/basler_ace_gige_c-mount_v01.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/basler_ace_gige_c-mount_v01.stl
new file mode 100644
index 000000000..01531abb9
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/basler_ace_gige_c-mount_v01.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery.stl
new file mode 100644
index 000000000..9cfcaab81
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_cage.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_cage.stl
new file mode 100644
index 000000000..6bfb9bbdd
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_cage.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_clip.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_clip.stl
new file mode 100644
index 000000000..6c97c8b9b
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/battery_clip.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_lower_basler_wolfgang_imu_v2.2.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_lower_basler_wolfgang_imu_v2.2.stl
new file mode 100644
index 000000000..b2a928ecc
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_lower_basler_wolfgang_imu_v2.2.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_left.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_left.stl
new file mode 100644
index 000000000..74f0de193
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_left.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_right.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_right.stl
new file mode 100644
index 000000000..9e3476d5f
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/camera_side_basler_wolfgang_v2.2_right.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/connector_shoulder.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/connector_shoulder.stl
new file mode 100644
index 000000000..5d9ceaf2e
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/connector_shoulder.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/core.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/core.stl
new file mode 100644
index 000000000..e778fb9be
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/core.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/dummy_speaker.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/dummy_speaker.stl
new file mode 100644
index 000000000..4c9f147f4
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/dummy_speaker.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/flex_stollen.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/flex_stollen.stl
new file mode 100644
index 000000000..21c0952af
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/flex_stollen.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_cover.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_cover.stl
new file mode 100644
index 000000000..d59c6418a
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_cover.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_left.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_left.stl
new file mode 100644
index 000000000..f6d0adddc
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_left.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_right.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_right.stl
new file mode 100644
index 000000000..8826d1214
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/foot_printed_right.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hand.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hand.stl
new file mode 100644
index 000000000..c8ea9fdb4
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hand.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hip_u_connector.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hip_u_connector.stl
new file mode 100644
index 000000000..561db981e
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/hip_u_connector.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/imu_holder.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/imu_holder.stl
new file mode 100644
index 000000000..c04c67f34
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/imu_holder.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_connector.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_connector.stl
new file mode 100644
index 000000000..121793261
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_connector.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_spacer.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_spacer.stl
new file mode 100644
index 000000000..247ca07fe
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/knee_spacer.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lense.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lense.stl
new file mode 100644
index 000000000..40d74deaa
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lense.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/load_cell.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/load_cell.stl
new file mode 100644
index 000000000..10a6ded35
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/load_cell.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_arm.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_arm.stl
new file mode 100644
index 000000000..a6ff1783b
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_arm.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg.stl
new file mode 100644
index 000000000..f251cda1c
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg_spacer.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg_spacer.stl
new file mode 100644
index 000000000..6bda6802f
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/lower_leg_spacer.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/motor_connector.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/motor_connector.stl
new file mode 100644
index 000000000..11930f160
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/motor_connector.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-106_body.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-106_body.stl
new file mode 100644
index 000000000..d52c169f7
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-106_body.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-64-body.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-64-body.stl
new file mode 100644
index 000000000..bc27727c1
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/mx-64-body.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_back.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_back.stl
new file mode 100644
index 000000000..19d998c09
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_back.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_front.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_front.stl
new file mode 100644
index 000000000..ae2851d5b
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_left_front.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_back.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_back.stl
new file mode 100644
index 000000000..b127ccabd
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_back.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_front.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_front.stl
new file mode 100644
index 000000000..2911ed855
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_holder_right_front.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_main.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_main.stl
new file mode 100644
index 000000000..773dc5a2d
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/nuc_main.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_connector.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_connector.stl
new file mode 100644
index 000000000..6330b8215
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_connector.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_ninjaflex.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_ninjaflex.stl
new file mode 100644
index 000000000..f133c5361
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/sea_ninjaflex.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/shoulder_connector.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/shoulder_connector.stl
new file mode 100644
index 000000000..a40dfeeed
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/shoulder_connector.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/speaker_holder.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/speaker_holder.stl
new file mode 100644
index 000000000..047963389
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/speaker_holder.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_lower.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_lower.stl
new file mode 100644
index 000000000..799e36c11
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_lower.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_upper.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_upper.stl
new file mode 100644
index 000000000..dfd73887a
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/spring_holder_upper.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_bottom.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_bottom.stl
new file mode 100644
index 000000000..058d88c8e
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_bottom.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_new.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_new.stl
new file mode 100644
index 000000000..ba6d42cba
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/springholder_new.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/thrustbearingholder.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/thrustbearingholder.stl
new file mode 100644
index 000000000..37c728c3c
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/thrustbearingholder.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bottom.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bottom.stl
new file mode 100644
index 000000000..229d6e0f6
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bottom.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_left.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_left.stl
new file mode 100644
index 000000000..1abead395
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_left.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_right.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_right.stl
new file mode 100644
index 000000000..96328fe4d
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_bumper_right.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_top.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_top.stl
new file mode 100644
index 000000000..681394317
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/torso_top.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm.stl
new file mode 100644
index 000000000..6c42126dd
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm_spacer.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm_spacer.stl
new file mode 100644
index 000000000..ee3be5ab4
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_arm_spacer.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg.stl
new file mode 100644
index 000000000..93698c335
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg_spacer.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg_spacer.stl
new file mode 100644
index 000000000..8472919b7
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/upper_leg_spacer.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/assets/xh-540.stl b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/xh-540.stl
new file mode 100644
index 000000000..5e6fb87b6
Binary files /dev/null and b/bitbots_simulation/bitbots_mujoco_sim/xml/assets/xh-540.stl differ
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/goal.xml b/bitbots_simulation/bitbots_mujoco_sim/xml/goal.xml
new file mode 100644
index 000000000..7b38f97ee
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/xml/goal.xml
@@ -0,0 +1,40 @@
+<mujoco model="soccer_goal">
+  <worldbody>
+        <body name="goal_a_frame" pos="7 0 0" euler="0 0 1.570796327"> 
+            <geom type="cylinder" size="0.05 0.9" pos="-1.3 0 0.9" rgba="1 1 1 1"/> <!-- left vertical bar front -->
+            <geom type="cylinder" size="0.05 0.9" pos="1.3 0 0.9" rgba="1 1 1 1"/> <!-- right vertical bar front -->
+            <geom type="cylinder" fromto="-1.3 0 1.8 -1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/> <!-- left vertical bar back-->
+            <geom type="cylinder" fromto="1.3 0 1.8 1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/> <!-- right vertical bar back -->
+            <geom type="cylinder" size="0.05 1.3" pos="0 0 1.8" euler="0 1.570796327 0" rgba="1 1 1 1"/> <!-- -->
+            <geom type="cylinder" fromto="-1.3 0 0.05 -1.3 -0.6 0.05" size="0.05" rgba="1 1 1 1"/> <!-- -->
+            <geom type="cylinder" fromto="1.3 0 0.05 1.3 -0.6 0.05" size="0.05" rgba="1 1 1 1"/> <!-- -->
+            <geom type="cylinder" size="0.05 0.9" pos="-1.3 -0.6 0.9" rgba="1 1 1 1"/> <!-- -->
+            <geom type="cylinder" size="0.05 0.9" pos="1.3 -0.6 0.9" rgba="1 1 1 1"/> <!-- -->
+            <geom type="cylinder" size="0.05 1.3" pos="0 -0.6 1.8" euler="0 1.570796327 0" rgba="1 1 1 1"/><!-- -->
+            <geom type="cylinder" fromto="-1.3 0 1.8 -1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/> <!---->
+            <geom type="cylinder" fromto="1.3 0 1.8 1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/> <!-- -->
+
+            <geom type="box" size="1.3 0.005 0.9" pos="0 -0.6 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="0.005 0.3 0.9" pos="1.3 -0.3 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="0.005 0.3 0.9" pos="-1.3 -0.3 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="1.3 0.3 0.005" pos="0 -0.3 1.8" rgba="1 1 1 0.3"/>
+        </body>
+        <body name="goal_b_frame" pos="-7 0 0" euler="0 0 -1.570796327">
+            <geom type="cylinder" size="0.05 0.9" pos="-1.3 0 0.9" rgba="1 1 1 1"/>
+            <geom type="cylinder" size="0.05 0.9" pos="1.3 0 0.9" rgba="1 1 1 1"/>
+            <geom type="cylinder" size="0.05 1.3" pos="0 0 1.8" euler="0 1.570796327 0" rgba="1 1 1 1"/>
+            <geom type="cylinder" fromto="-1.3 0 0.05 -1.3 -0.6 0.05" size="0.05" rgba="1 1 1 1"/>
+            <geom type="cylinder" fromto="1.3 0 0.05 1.3 -0.6 0.05" size="0.05" rgba="1 1 1 1"/>
+            <geom type="cylinder" size="0.05 0.9" pos="-1.3 -0.6 0.9" rgba="1 1 1 1"/>
+            <geom type="cylinder" size="0.05 0.9" pos="1.3 -0.6 0.9" rgba="1 1 1 1"/>
+            <geom type="cylinder" size="0.05 1.3" pos="0 -0.6 1.8" euler="0 1.570796327 0" rgba="1 1 1 1"/>
+            <geom type="cylinder" fromto="-1.3 0 1.8 -1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/>
+            <geom type="cylinder" fromto="1.3 0 1.8 1.3 -0.6 1.8" size="0.05" rgba="1 1 1 1"/>
+
+            <geom type="box" size="1.3 0.005 0.9" pos="0 -0.6 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="0.005 0.3 0.9" pos="1.3 -0.3 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="0.005 0.3 0.9" pos="-1.3 -0.3 0.9" rgba="1 1 1 0.3"/>
+            <geom type="box" size="1.3 0.3 0.005" pos="0 -0.3 1.8" rgba="1 1 1 0.3"/>
+        </body>
+  </worldbody>
+</mujoco>
diff --git a/bitbots_simulation/bitbots_mujoco_sim/xml/wolfgang.xml b/bitbots_simulation/bitbots_mujoco_sim/xml/wolfgang.xml
new file mode 100644
index 000000000..357dfdeee
--- /dev/null
+++ b/bitbots_simulation/bitbots_mujoco_sim/xml/wolfgang.xml
@@ -0,0 +1,669 @@
+<mujoco model="wolfgang">
+  <compiler angle="radian" autolimits="true"/>
+
+  <option iterations="3" ls_iterations="5" timestep="0.002" integrator="Euler">
+    <flag eulerdamp="disable"/>
+  </option>
+
+  <default>
+    <default class="humanoid">
+      <default class="collision">
+        <geom group="3" contype="1" conaffinity="1" />
+      </default>
+      <default class="visual">
+        <geom material="black" contype="0" conaffinity="0" group="2" />
+      </default>
+      <default class="mx106">
+        <joint damping="1.7" armature="0.025" frictionloss="0.1" />
+        <position kp="45.0" ctrlrange="-3.141592 3.141592" forcerange="-12 12" />
+      </default>
+      <default class="mx64">
+        <joint damping="0.66" armature="0.012" frictionloss="0.09" />
+        <position kp="25.5" ctrlrange="-3.141592 3.141592" forcerange="-12 12" />
+      </default>
+      <default class="xh-540">
+        <joint damping="1.7" armature="0.025" frictionloss="0.1" />
+        <position kp="45.0" ctrlrange="-3.141592 3.141592" forcerange="-12 12" />
+      </default>
+      <site size="0.01" rgba="1 0 0 1" group="4"/>
+    </default>
+  </default>
+
+  <asset>
+    <mesh name="mx-64-body" file="mx-64-body.stl"/>
+    <mesh name="nuc_holder_right_front" file="nuc_holder_right_front.stl"/>
+    <mesh name="torso_bumper_right" file="torso_bumper_right.stl"/>
+    <mesh name="core" file="core.stl"/>
+    <mesh name="battery" file="battery.stl"/>
+    <mesh name="mx-106_body" file="mx-106_body.stl"/>
+    <mesh name="torso_bumper_left" file="torso_bumper_left.stl"/>
+    <mesh name="nuc_holder_right_back" file="nuc_holder_right_back.stl"/>
+    <mesh name="battery_clip" file="battery_clip.stl"/>
+    <mesh name="nuc_holder_left_back" file="nuc_holder_left_back.stl"/>
+    <mesh name="imu_holder" file="imu_holder.stl"/>
+    <mesh name="torso_bottom" file="torso_bottom.stl"/>
+    <mesh name="speaker_holder" file="speaker_holder.stl"/>
+    <mesh name="nuc_main" file="nuc_main.stl"/>
+    <mesh name="torso_top" file="torso_top.stl"/>
+    <mesh name="baseplate_odroid_xu4_core" file="baseplate_odroid_xu4_core.stl"/>
+    <mesh name="dummy_speaker" file="dummy_speaker.stl"/>
+    <mesh name="nuc_holder_left_front" file="nuc_holder_left_front.stl"/>
+    <mesh name="battery_cage" file="battery_cage.stl"/>
+    <mesh name="sea_connector" file="sea_connector.stl"/>
+    <mesh name="sea_ninjaflex" file="sea_ninjaflex.stl"/>
+    <mesh name="shoulder_connector" file="shoulder_connector.stl"/>
+    <mesh name="upper_arm" file="upper_arm.stl"/>
+    <mesh name="connector_shoulder" file="connector_shoulder.stl"/>
+    <mesh name="upper_arm_spacer" file="upper_arm_spacer.stl"/>
+    <mesh name="lower_arm" file="lower_arm.stl"/>
+    <mesh name="hand" file="hand.stl"/>
+    <mesh name="hip_u_connector" file="hip_u_connector.stl"/>
+    <mesh name="motor_connector" file="motor_connector.stl"/>
+    <mesh name="thrustbearingholder" file="thrustbearingholder.stl"/>
+    <mesh name="knee_connector" file="knee_connector.stl"/>
+    <mesh name="upper_leg" file="upper_leg.stl"/>
+    <mesh name="xh-540" file="xh-540.stl"/>
+    <mesh name="spring_holder_upper" file="spring_holder_upper.stl"/>
+    <mesh name="upper_leg_spacer" file="upper_leg_spacer.stl"/>
+    <mesh name="knee_spacer" file="knee_spacer.stl"/>
+    <mesh name="lower_leg" file="lower_leg.stl"/>
+    <mesh name="lower_leg_spacer" file="lower_leg_spacer.stl"/>
+    <mesh name="springholder_new" file="springholder_new.stl"/>
+    <mesh name="spring_holder_lower" file="spring_holder_lower.stl"/>
+    <mesh name="springholder_bottom" file="springholder_bottom.stl"/>
+    <mesh name="load_cell" file="load_cell.stl"/>
+    <mesh name="ankle" file="ankle.stl"/>
+    <mesh name="foot_printed_right" file="foot_printed_right.stl"/>
+    <mesh name="foot_cover" file="foot_cover.stl"/>
+    <mesh name="flex_stollen" file="flex_stollen.stl"/>
+    <mesh name="foot_printed_left" file="foot_printed_left.stl"/>
+    <mesh name="lense" file="lense.stl"/>
+    <mesh name="camera_side_basler_wolfgang_v2.2_right" file="camera_side_basler_wolfgang_v2.2_right.stl"/>
+    <mesh name="camera_side_basler_wolfgang_v2.2_left" file="camera_side_basler_wolfgang_v2.2_left.stl"/>
+    <mesh name="basler_ace_gige_c-mount_v01" file="basler_ace_gige_c-mount_v01.stl"/>
+    <mesh name="camera_lower_basler_wolfgang_imu_v2.2" file="camera_lower_basler_wolfgang_imu_v2.2.stl"/>
+    <material name="black" rgba="0.2 0.2 0.2 1"/>
+  </asset>
+
+  <worldbody>
+    <body name="torso" pos="0.0 0.0 0.4274" quat="0.999 0.0 0.05 0.0" childclass="humanoid" >
+      <freejoint/>
+      <site name="imu"/>
+      <inertial pos="-0.00564376 -0.0026297 0.109553" quat="0.997756 0.00472747 0.0649963 -0.0153334" mass="3.0839" diaginertia="0.0185762 0.0158759 0.00761796"/>
+      <geom pos="-0.0015 0.053 0.2035" quat="9.38184e-07 -0.707105 9.38187e-07 0.707108" type="mesh" mesh="mx-64-body" class="visual"/>
+      <geom size="0.0201 0.0205 0.03055" pos="0.01605 0.053 0.2035" quat="9.38184e-07 -0.707105 9.38187e-07 0.707108" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.0015 -0.053 0.2035" quat="0.707105 0 -0.707108 0" type="mesh" mesh="mx-64-body" class="visual"/>
+      <geom size="0.0201 0.0205 0.03055" pos="0.01605 -0.053 0.2035" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.01485 -0.059 0.152" quat="0.707105 0 0 0.707108" type="mesh" mesh="nuc_holder_right_front" class="visual"/>
+      <geom pos="-0.057 0.077 0.213915" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="torso_bumper_right" class="visual"/>
+      <geom size="0.0095 0.0365 0.0065" pos="-0.0635 0.0675 0.177415" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="-0.0831853 0.0675 0.193936" quat="0.30209 0.639328 0.63933 0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="-0.0831853 0.0675 0.160894" quat="0.30209 -0.639328 -0.63933 0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.01 0.001 0.118" quat="0.707105 0 0 -0.707108" type="mesh" mesh="core" class="visual"/>
+      <geom pos="-0.0175 -0.0035 0.0295" quat="0.707105 0 0 -0.707108" type="mesh" mesh="battery" class="visual"/>
+      <geom size="0.07 0.02 0.022" pos="-0.0375 -0.0035 0.0295" quat="0.707105 0 0 -0.707108" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="0 -0.055 0.026" quat="0.499998 0.5 -0.500002 -0.5" type="mesh" mesh="mx-106_body" class="visual"/>
+      <geom size="0.0201 0.023 0.03255" pos="0.01805 -0.055 0.026" quat="0.499998 0.5 -0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="0.053 0.077 0.213915" quat="0.499998 -0.5 0.500002 -0.5" type="mesh" mesh="torso_bumper_left" class="visual"/>
+      <geom size="0.0095 0.0365 0.0065" pos="0.0595 0.0675 0.177415" quat="0.499998 -0.5 0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="0.0791853 0.0675 0.193936" quat="0.30209 0.639328 -0.63933 -0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="0.0791853 0.0675 0.160894" quat="0.30209 -0.639328 0.63933 -0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.04715 -0.0589 0.152" quat="0.707105 0 0 0.707108" type="mesh" mesh="nuc_holder_right_back" class="visual"/>
+      <geom pos="0 0.055 0.026" quat="0.499998 0.5 -0.500002 -0.5" type="mesh" mesh="mx-106_body" class="visual"/>
+      <geom size="0.0201 0.023 0.03255" pos="0.01805 0.055 0.026" quat="0.499998 0.5 -0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.0375 -0.00925 0.0295" quat="0.707105 0 0 0.707108" type="mesh" mesh="battery_clip" class="visual"/>
+      <geom pos="-0.04715 0.0359 0.149" quat="0.707105 0 0 0.707108" type="mesh" mesh="nuc_holder_left_back" class="visual"/>
+      <geom pos="-0.0015 -0.036 0.2035" quat="9.38184e-07 -0.707105 9.38187e-07 0.707108" type="mesh" mesh="imu_holder" class="visual"/>
+      <geom pos="-0.062 0.075 0.003" type="mesh" mesh="torso_bottom" class="visual"/>
+      <geom size="0.06 0.075 0.04" pos="-0.002 0 0.043" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.0095 0 0.211" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="mx-64-body" class="visual"/>
+      <geom size="0.0201 0.0205 0.03055" pos="0.00805 0 0.211" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.057 -0.077 0.213915" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="torso_bumper_left" class="visual"/>
+      <geom size="0.0095 0.065 0.0065" pos="-0.0635 -0.0675 0.177415" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="-0.0831853 -0.0675 0.193936" quat="0.30209 0.639328 0.63933 0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="-0.0831853 -0.0675 0.160894" quat="0.30209 -0.639328 -0.63933 0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="0.03 0 0.006" quat="0.707105 0 0 0.707108" type="mesh" mesh="speaker_holder" class="visual"/>
+      <geom pos="-0.002 -0.0115 0.152" quat="0.707105 0 0 0.707108" type="mesh" mesh="nuc_main" class="visual"/>
+      <geom pos="0.053 -0.077 0.213915" quat="0.499998 -0.5 0.500002 -0.5" type="mesh" mesh="torso_bumper_right" class="visual"/>
+      <geom size="0.0095 0.0365 0.0065" pos="0.0595 -0.0675 0.177415" quat="0.499998 -0.5 0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="0.0791853 -0.0675 0.193936" quat="0.30209 0.639328 -0.63933 -0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.0095 0.0234397 0.0025" pos="0.0791853 -0.0675 0.160894" quat="0.30209 -0.639328 0.63933 -0.302091" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="-0.077 -0.075 0.083" quat="0.707105 0 0 0.707108" type="mesh" mesh="torso_top" class="visual"/>
+      <geom pos="-0.002 0 0.088" quat="0.707105 0 0 -0.707108" type="mesh" mesh="baseplate_odroid_xu4_core" class="visual"/>
+      <geom size="0.075 0.0625 0.0575" pos="-0.0045 0 0.1405" quat="0.707105 0 0 0.707108" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.075 0.005 0.0273829" pos="0.053 0 0.115383" quat="0.706352 0.0326317 0.0326318 0.706355" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.075 0.005 0.0327252" pos="0.058 0 0.175725" quat="0.707105 0 0 0.707108" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom size="0.075 0.068 0.0175" pos="-0.005 0 0.2155" quat="0.707105 0 0 0.707108" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+      <geom pos="0.0314886 0 0.011" quat="0.707105 0 0 0.707108" type="mesh" mesh="dummy_speaker" class="visual"/>
+      <geom pos="-0.01485 0.036 0.149" quat="0.707105 0 0 0.707108" type="mesh" mesh="nuc_holder_left_front" class="visual"/>
+      <geom pos="-0.0375 -0.00925 0.0295" quat="0.707105 0 0 0.707108" type="mesh" mesh="battery_cage" class="visual"/>
+      <body name="r_shoulder" pos="-0.0015 -0.0765 0.2035" quat="1.32679e-06 0 0 -1">
+        <inertial pos="-0.0135397 0.0284022 -1.54699e-06" quat="0.180501 0.68368 -0.180422 0.683703" mass="0.041445" diaginertia="3.49573e-05 3.19948e-05 1.42174e-05" />
+        <joint name="RShoulderPitch" pos="0 0 0" axis="0 1 0" range="-3.14159 3.14159" class="mx106"/>
+        <geom pos="-0.02595 0.042 0" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="sea_connector" class="visual"/>
+        <geom size="0.025 0.0025" pos="-0.02845 0.042 0" quat="0.499998 -0.5 -0.500002 0.5" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+        <geom pos="-0.01695 0.042 0" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="sea_ninjaflex" class="visual"/>
+        <geom size="0.025 0.0045" pos="-0.02145 0.042 0" quat="0.499998 -0.5 -0.500002 0.5" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+        <geom type="mesh" mesh="shoulder_connector" class="visual"/>
+        <geom size="0.03195 0.00075 0.017" pos="0 0.00075 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+        <geom size="0.00075 0.0225 0.014" pos="0.0312 0.0225 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+        <geom size="0.00075 0.0225 0.014" pos="-0.0312 0.0225 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+        <body name="r_upper_arm" pos="-0.01695 0.042 0" quat="0.499998 0.5 -0.500002 -0.5">
+          <inertial pos="9.77435e-05 -0.0325821 -0.0239229" quat="0.493157 0.506993 -0.500259 0.499495" mass="0.228498" diaginertia="0.000371168 0.000241466 0.00017944" />
+          <joint name="RShoulderRoll" pos="0 0 0" axis="0 0 -1" range="0 3.14159" class="mx106"/>
+          <geom pos="0 0 -0.0235" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-64-body" class="visual"/>
+          <geom size="0.0201 0.0205 0.03055" pos="0 -0.01755 -0.0235" quat="0.707105 -0.707108 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom pos="0.0265 -0.144 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="upper_arm" class="visual"/>
+          <geom size="0.02 0.001125" pos="0.025125 -0.144 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom size="0.02 0.001125" pos="0.025125 -0.014 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom size="0.02 0.001125 0.065" pos="0.025125 -0.079 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom pos="-0.0147 -0.0105 -0.0235" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="connector_shoulder" class="visual"/>
+          <geom size="0.018 0.02 0.00465" pos="-0.01935 -0.0105 -0.0235" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom pos="-0.0265 -0.144 -0.0235" quat="0.499998 -0.5 0.5 0.500002" type="mesh" mesh="upper_arm" class="visual"/>
+          <geom size="0.02 0.001125" pos="-0.025125 -0.144 -0.0235" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom size="0.02 0.001125" pos="-0.025125 -0.014 -0.0235" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom size="0.02 0.001125 0.065" pos="-0.025125 -0.079 -0.0235" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom pos="0.0147 -0.0105 -0.0235" quat="0.499998 0.5 0.500002 0.5" type="mesh" mesh="connector_shoulder" class="visual"/>
+          <geom size="0.018 0.02 0.00465" pos="0.01935 -0.0105 -0.0235" quat="0.499998 0.5 0.500002 0.5" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+          <geom pos="0.024 -0.064 -0.038499" quat="0.707105 0 -0.707108 0" type="mesh" mesh="upper_arm_spacer" class="visual"/>
+          <geom size="0.0188 0.0115 0.023" pos="0.001 -0.0715 -0.023699" quat="0.707105 0 -0.707108 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+            <body name="r_lower_arm" pos="0.024 -0.144 -0.0235" quat="0.499998 -0.5 0.500002 -0.5">
+            <inertial pos="0.000436307 -0.0605126 -0.0235553" quat="0.500185 0.49962 -0.500396 0.499799" mass="0.207774" diaginertia="0.00110076 0.001088 8.9031e-05" />
+            <joint name="RElbow" pos="0 0 0" axis="0 0 1" range="-1.0472 1.5708" class="mx64"/>
+            <geom pos="0 -0.163682 -0.0065" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" mesh="lower_arm" class="visual"/>
+            <geom size="0.04 0.001" pos="0 -0.163682 -0.0055" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.022 0.001 0.0725" pos="0.0015 -0.0631823 -0.0055" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.001" pos="0 -0.193682 -0.0055" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="-0.0282 -0.187682 -0.0055" quat="0.701469 -0.701472 -0.0891014 0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="0.0282 -0.187682 -0.0055" quat="0.701469 -0.701472 0.0891014 -0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 0 -0.0235" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-64-body" class="visual"/>
+            <geom size="0.0201 0.0205 0.03055" pos="0 -0.01755 -0.0235" quat="0.707105 -0.707108 0 0" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 -0.163682 -0.0065" quat="0.707105 0.707108 0 0" type="mesh" mesh="hand" class="visual"/>
+            <geom size="0.04 0.017" pos="0 -0.163682 -0.0235" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.017" pos="0 -0.193682 -0.0235" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.017 0.0145" pos="0.0282 -0.187682 -0.0235" quat="0.0891027 0.089103 0.701472 0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.017 0.0145" pos="-0.0282 -0.187682 -0.0235" quat="0.0891027 0.089103 -0.701472 -0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 -0.163682 -0.0425" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" mesh="lower_arm" class="visual"/>
+            <geom size="0.04 0.001" pos="0 -0.163682 -0.0415" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.022 0.001 0.0725" pos="0.0015 -0.0631823 -0.0415" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.001" pos="0 -0.193682 -0.0415" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="-0.0282 -0.187682 -0.0415" quat="0.701469 -0.701472 -0.0891014 0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="0.0282 -0.187682 -0.0415" quat="0.701469 -0.701472 0.0891014 -0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+          </body>
+        </body>
+      </body>
+      <body name="l_shoulder" pos="-0.0015 0.0765 0.2035" quat="1.7603e-12 -1.32679e-06 -1 1.32679e-06">
+        <inertial pos="-0.0135397 0.0284022 1.54699e-06" quat="0.180422 0.683703 -0.180501 0.68368" mass="0.041445" diaginertia="3.49573e-05 3.19948e-05 1.42174e-05" />
+        <joint name="LShoulderPitch" pos="0 0 0" axis="0 1 0" range="-3.14159 3.14159" class="mx106"/>
+        <geom pos="-0.02595 0.042 0" quat="0.499998 0.5 -0.500002 -0.5" type="mesh" mesh="sea_connector" class="visual"/>
+        <geom size="0.025 0.0025" pos="-0.02845 0.042 0" quat="0.499998 0.5 -0.500002 -0.5" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom type="mesh" mesh="shoulder_connector" class="visual"/>
+        <geom size="0.03195 0.00075 0.017" pos="0 0.00075 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.00075 0.0225 0.014" pos="0.0312 0.0225 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.00075 0.0225 0.014" pos="-0.0312 0.0225 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom pos="-0.01695 0.042 0" quat="0.499998 0.5 -0.500002 -0.5" type="mesh" mesh="sea_ninjaflex" class="visual"/>
+        <geom size="0.025 0.0045" pos="-0.02145 0.042 0" quat="0.499998 0.5 -0.500002 -0.5" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <body name="l_upper_arm" pos="-0.01695 0.042 0" quat="0.499998 -0.5 -0.500002 0.5">
+          <inertial pos="9.77435e-05 -0.0325821 -0.0239229" quat="0.493157 0.506993 -0.500259 0.499495" mass="0.228498" diaginertia="0.000371168 0.000241466 0.00017944" />
+          <joint name="LShoulderRoll" pos="0 0 0" axis="0 0 -1" range="-3.14159 0" class="mx106"/>
+          <geom pos="0 0 -0.0235" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-64-body" class="visual"/>
+          <geom size="0.0201 0.0205 0.03055" pos="0 -0.01755 -0.0235" quat="0.707105 -0.707108 0 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="-0.0147 -0.0105 -0.0235" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="connector_shoulder" class="visual"/>
+          <geom size="0.018 0.02 0.00465" pos="-0.01935 -0.0105 -0.0235" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.0147 -0.0105 -0.0235" quat="0.499998 0.5 0.500002 0.5" type="mesh" mesh="connector_shoulder" class="visual"/>
+          <geom size="0.018 0.02 0.00465" pos="0.01935 -0.0105 -0.0235" quat="0.499998 0.5 0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.024 -0.064 -0.038499" quat="0.707105 0 -0.707108 0" type="mesh" mesh="upper_arm_spacer" class="visual"/>
+          <geom size="0.0188 0.0115 0.023" pos="0.001 -0.0715 -0.023699" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="-0.0265 -0.144 -0.0235" quat="0.499998 -0.5 0.5 0.500002" type="mesh" mesh="upper_arm" class="visual"/>
+          <geom size="0.02 0.001125" pos="-0.025125 -0.144 -0.0235" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001125" pos="-0.025125 -0.014 -0.0235" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001125 0.065" pos="-0.025125 -0.079 -0.0235" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.0265 -0.144 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="upper_arm" class="visual"/>
+          <geom size="0.02 0.001125" pos="0.025125 -0.144 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001125" pos="0.025125 -0.014 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001125 0.065" pos="0.025125 -0.079 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <body name="l_lower_arm" pos="-0.024 -0.144 -0.0235" quat="0.499998 -0.5 -0.500002 0.5">
+            <inertial pos="-0.000436306 -0.0605126 -0.0235553" quat="0.502963 0.497233 -0.502767 0.497003" mass="0.207774" diaginertia="0.00110076 0.00108813 8.89004e-05" />
+            <joint name="LElbow" pos="0 0 0" axis="0 0 1" range="-1.5708 1.0472" class="mx64"/>
+            <geom pos="0 0 -0.0235" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-64-body" class="visual"/>
+            <geom size="0.0201 0.0205 0.03055" pos="0 -0.01755 -0.0235" quat="0.707105 -0.707108 0 0" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 -0.163682 -0.0405" quat="0.707105 0.707108 0 0" type="mesh" mesh="lower_arm" class="visual"/>
+            <geom size="0.04 0.001" pos="0 -0.163682 -0.0415" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.022 0.001 0.0725" pos="-0.0015 -0.0631823 -0.0415" quat="0.707105 0.707108 0 0" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.001" pos="0 -0.193682 -0.0415" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="0.0282 -0.187682 -0.0415" quat="0.0891027 0.089103 0.701472 0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="-0.0282 -0.187682 -0.0415" quat="0.0891027 0.089103 -0.701472 -0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 -0.163682 -0.0405" quat="9.38184e-07 -9.38187e-07 0.707108 -0.707105" type="mesh" mesh="hand" class="visual"/>
+            <geom size="0.04 0.017" pos="0 -0.163682 -0.0235" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.017" pos="0 -0.193682 -0.0235" quat="1.32679e-06 0 0 -1" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.017 0.0145" pos="-0.0282 -0.187682 -0.0235" quat="0.701469 -0.701472 -0.0891014 0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.017 0.0145" pos="0.0282 -0.187682 -0.0235" quat="0.701469 -0.701472 0.0891014 -0.089101" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom pos="0 -0.163682 -0.0045" quat="0.707105 0.707108 0 0" type="mesh" mesh="lower_arm" class="visual"/>
+            <geom size="0.04 0.001" pos="0 -0.163682 -0.0055" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.022 0.001 0.0725" pos="-0.0015 -0.0631823 -0.0055" quat="0.707105 0.707108 0 0" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.032 0.001" pos="0 -0.193682 -0.0055" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="0.0282 -0.187682 -0.0055" quat="0.0891027 0.089103 0.701472 0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+            <geom size="0.006 0.001 0.0145" pos="-0.0282 -0.187682 -0.0055" quat="0.0891027 0.089103 -0.701472 -0.701469" type="box" rgba="0.960784 0.960784 0.960784 1" class="collision"/>
+          </body>
+        </body>
+      </body>
+      <body name="r_hip_1" pos="0 -0.055 0" quat="0.707105 -0.707108 0 0">
+        <inertial pos="1.20984e-11 0.010315 -1.06799e-07" quat="-6.86297e-06 0.707107 -6.86297e-06 0.707107" mass="0.0689735" diaginertia="0.000110523 0.00010714 2.18955e-05" />
+        <joint name="RHipYaw" pos="0 0 0" axis="0 1 0" range="-1.5708 1.5708" class="mx106"/>
+        <geom pos="0 -0.0006 -0.018" type="mesh" mesh="hip_u_connector" class="visual"/>
+        <geom size="0.018 0.05 0.002" pos="0 0.0014 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.0130115 0.002 0.0265" pos="-0.048 0.0259 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.0130115 0.002 0.0265" pos="0.048 0.0259 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.025 0.002" pos="0 -0.0006 0.002" quat="0.499998 -0.5 0.5 0.500002" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <body name="r_hip_2" pos="-0.046 0.0414 0" quat="9.38184e-07 -0.707105 9.38187e-07 0.707108">
+          <inertial pos="1.38724e-05 -0.0155281 -0.0486407" quat="0.99678 0.0129755 -0.00329376 0.0790604" mass="0.319807" diaginertia="0.000268643 0.000268139 0.000121226" />
+          <joint name="RHipRoll" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" class="mx106"/> 
+          <geom pos="0 0 -0.026" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-106_body" class="visual"/>
+          <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.026" quat="0.707105 -0.707108 0 0" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <geom pos="-0.0145 -0.021 -0.043" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="mesh" mesh="motor_connector" class="visual"/>
+          <geom size="0.014 0.003 0.00285" pos="-0.0175 -0.007 -0.04585" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <geom size="0.014 0.00175 0.00295" pos="-0.01875 -0.007 -0.05165" quat="9.38184e-07 -0.707105 -0.707108 9.38187e-07" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <geom pos="0.014 0.0105 -0.0892" quat="1.32679e-06 0 0 -1" type="mesh" mesh="thrustbearingholder" class="visual"/>
+          <geom pos="0.0145 0.007 -0.043" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="mesh" mesh="motor_connector" class="visual"/>
+          <geom size="0.014 0.003 0.00285" pos="0.0175 -0.007 -0.04585" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <geom size="0.014 0.00175 0.00295" pos="0.01875 -0.007 -0.05165" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <geom pos="0 0 -0.0691" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="mx-106_body" class="visual"/>
+          <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.0691" quat="0.499998 -0.5 -0.5 -0.500002" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+          <body name="r_upper_leg" pos="-0.0265 0 -0.0691" quat="0.499998 -0.5 0.5 0.500002">
+            <inertial pos="0.00493985 -0.0283223 -0.133144" quat="0.995752 -0.00919328 -0.0209573 0.0891915" mass="0.242311" diaginertia="0.000503914 0.000457532 0.000112612" />
+            <joint name="RHipPitch" pos="0 0 0" axis="0 1 0" range="-2.0944 1.91986" class="mx106"/>
+            <geom pos="0.0182355 -0.0226528 -0.130164" quat="0.183014 0.183012 -0.683014 0.683011" type="mesh" mesh="knee_connector" class="visual"/>
+            <geom size="0.021 0.0089913 0.01225" pos="0.0157399 -0.04075 -0.134486" quat="0.183014 0.183012 -0.683014 0.683011" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.0124063 0.002" pos="0.0140324 -0.051 -0.137444" quat="0.183014 0.183012 -0.683014 0.683011" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0 0.002 0" type="mesh" mesh="upper_leg" class="visual"/>
+            <geom size="0.018 0.001" pos="0 0.001 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.01 0.001" pos="0.009329 0.001 -0.143" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="0.0148403 0.001 -0.132659" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.019 0.001 0.0510495" pos="0.00259365 0.001 -0.0830903" quat="0.991445 0 -0.130526 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.005 0.001 0.005" pos="0.00161489 0.001 -0.0253454" quat="0.130526 1.14226e-06 0.991445 1.14226e-06" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.01 0.001" pos="0.009329 0.001 -0.143" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.00400978 0.001 -0.00242195" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.00770728 0.001 -0.00682845" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0105834 0.001 -0.0118101" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0125508 0.001 -0.0172154" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0135497 0.001 -0.0228803" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0135497 0.001 -0.0286326" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0125508 0.001 -0.0342975" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.018 0.001" pos="-2.3657e-07 -0.052 -3.21056e-07" quat="0.183014 0.183012 -0.683014 0.683011" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="-0.00392295 -0.0265 -0.169043" quat="0.183014 0.183012 -0.683014 0.683011" type="mesh" mesh="xh-540" class="visual"/>
+            <geom size="0.017 0.02925 0.022" pos="0.00382705 -0.0265 -0.15562" quat="0.183014 0.183012 -0.683014 0.683011" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.014 -0.055 -0.138" quat="0.707105 0.707108 0 0" type="mesh" mesh="spring_holder_upper" class="visual"/>
+            <geom size="0.014 0.011" pos="0.014 -0.066 -0.138" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.018 0.001" pos="-2.3657e-07 -0.001 -3.21056e-07" quat="0.183014 0.183012 -0.683014 0.683011" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0 -0.053 0" quat="1 0 0 0" type="mesh" mesh="upper_leg" class="visual"/>
+            <geom size="0.018 0.001" pos="0 -0.054 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.01 0.001" pos="0.009329 -0.054 -0.143" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="0.0148403 -0.054 -0.132659" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.019 0.001 0.0510495" pos="0.00259365 -0.054 -0.0830903" quat="0.991445 0 -0.130526 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.005 0.001 0.005" pos="0.00161489 -0.054 -0.0253454" quat="0.130526 1.14226e-06 0.991445 1.14226e-06" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.01 0.001" pos="0.009329 -0.054 -0.143" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.00400978 -0.054 -0.00242195" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.00770728 -0.054 -0.00682845" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0105834 -0.054 -0.0118101" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0125508 -0.054 -0.0172154" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0135497 -0.054 -0.0228803" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0135497 -0.054 -0.0286326" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001" pos="-0.0125508 -0.054 -0.0342975" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.0186686 -0.0303472 -0.130414" quat="0.683011 -0.683014 0.183012 0.183013" type="mesh" mesh="knee_connector" class="visual"/>
+            <geom size="0.021 0.0089913 0.01225" pos="0.0161729 -0.01225 -0.134736" quat="0.683011 -0.683014 0.183012 0.183013" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.0124063 0.002" pos="0.0144654 -0.00200004 -0.137694" quat="0.683011 -0.683014 0.183012 0.183013" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.0220754 -0.053 -0.0939778" quat="0.0922969 -0.0922947 0.701059 0.701056" type="mesh" mesh="upper_leg_spacer" class="visual"/>
+            <geom size="0.017575 0.019 0.0265" pos="0.00281079 -0.0265 -0.0836106" quat="0.0922969 -0.0922947 0.701059 0.701056" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <body name="r_lower_leg" pos="-0.00392295 -0.051 -0.169043" quat="0.991445 0 -0.130526 0">
+              <inertial pos="0.00142428 0.0183569 -0.0617082" quat="0.996507 0.0805308 0.0213013 -0.00583843" mass="0.0880635" diaginertia="0.000284834 0.000243363 7.08604e-05" />
+              <joint name="RKnee" pos="0 0 0" axis="0 -1 0" range="-2.96706 0" class="xh-540" ref="0.15"/>
+              <geom pos="0.02797 0.068019 -0.15413" type="mesh" mesh="knee_spacer" class="visual"/>
+              <geom size="0.018 0.00075" pos="4.35337e-09 0.049 -2.49321e-07" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0 0.0525 -0.17" type="mesh" mesh="lower_leg" class="visual"/>
+              <geom size="0.018 0.001" pos="0 0.0515 -0.17" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.018 0.001" pos="0 0.0515 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.02 0.001" pos="-0.004 0.0515 -0.165" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.02 0.001" pos="0.004 0.0515 -0.005" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.017 0.001 0.08" pos="-9.2229e-05 0.0515 -0.085786" quat="0.999048 0 0.0436194 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0 -0.0015 -0.17" type="mesh" mesh="lower_leg" class="visual"/>
+              <geom size="0.018 0.001" pos="0 -0.0025 -0.17" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.018 0.001" pos="0 -0.0025 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.02 0.001" pos="-0.004 -0.0025 -0.165" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.02 0.001" pos="0.004 -0.0025 -0.005" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.017 0.001 0.08" pos="-9.2229e-05 -0.0025 -0.085786" quat="0.999048 0 0.0436194 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="-0.00951565 -0.0015 -0.03402" quat="0.477716 -0.477713 0.521335 0.521332" type="mesh" mesh="lower_leg_spacer" class="visual"/>
+              <geom size="0.0322425 0.01675 0.0255" pos="0.00253479 0.024 -0.0569148" quat="0.477716 -0.477713 0.521335 0.521332" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0 -0.0035 -0.17" type="mesh" mesh="springholder_new" class="visual"/>
+              <geom size="0.0165 0.0015" pos="0 -0.024 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom size="0.0105 0.00815" pos="0 -0.01435 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0.0202407 0.0033 0.0613528" quat="-0.0871558 1.43738e-06 0.996195 1.43738e-06" type="mesh" mesh="spring_holder_lower" class="visual"/>
+              <geom size="0.007 0.00455 0.0155" pos="0.0125977 -0.00805 -0.0413085" quat="-0.0436194 1.38341e-06 0.999048 1.38341e-06" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0.02797 0.017519 -0.15413" type="mesh" mesh="knee_spacer" class="visual"/>
+              <geom size="0.018 0.00075" pos="4.35337e-09 -0.00150002 -2.49321e-07" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <geom pos="0 -0.0035 0" type="mesh" mesh="springholder_bottom" class="visual"/>
+              <geom size="0.015 0.00135" pos="0 -0.0035 0.00135" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+              <body name="r_ankle" pos="0 0.0505 -0.17" quat="0.707105 -0.707108 0 0">
+                <inertial pos="-0.0204557 -0.0155281 -0.0260036" quat="-0.0712448 0.706639 -0.050439 0.702169" mass="0.319807" diaginertia="0.000272991 0.000268476 0.000116637" />
+              <joint name="RAnklePitch" pos="0 0 0" axis="0 0 -1" range="-1.74533 1.22173" class="mx106"/>
+                <geom pos="-0.0261 -0.021 -0.0405" quat="0.499998 0.5 0.500002 0.5" type="mesh" mesh="motor_connector" class="visual"/>
+                <geom size="0.014 0.003 0.00285" pos="-0.02325 -0.007 -0.0435" quat="0.499998 0.5 0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <geom size="0.014 0.00175 0.00295" pos="-0.01745 -0.007 -0.04475" quat="0.499998 0.5 0.500002 0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <geom pos="-0.0431 0 -0.026" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="mx-106_body" class="visual"/>
+                <geom size="0.0201 0.023 0.03255" pos="-0.0431 -0.01805 -0.026" quat="0.499998 -0.5 -0.5 -0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <geom pos="-0.0261 0.007 -0.0115" quat="0.499998 -0.5 0.500002 -0.5" type="mesh" mesh="motor_connector" class="visual"/>
+                <geom size="0.014 0.003 0.00285" pos="-0.02325 -0.007 -0.0085" quat="0.499998 -0.5 0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <geom size="0.014 0.00175 0.00295" pos="-0.01745 -0.007 -0.00725" quat="0.499998 -0.5 0.500002 -0.5" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <geom pos="0.0201 0.0105 -0.0109" quat="9.38184e-07 0.707105 9.38187e-07 -0.707108" type="mesh" mesh="thrustbearingholder" class="visual"/>
+                <geom pos="0 0 -0.026" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-106_body" class="visual"/>
+                <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.026" quat="0.707105 -0.707108 0 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+                <body name="r_foot_l" pos="-0.0691 0 -0.026" quat="0.505584 0.494353 0.494351 -0.505586">
+                  <inertial pos="0.0120466 0.0909614 -0.0301459" quat="0.51331 0.497032 -0.512829 0.475898" mass="0.274879" diaginertia="0.0012242 0.000943261 0.000330309" />
+                  <joint name="RAnkleRoll" pos="0 0 0" axis="0 -1 0" range="-1.0472 1.0472" class="mx106"/>
+                  <site name="r_foot"/>
+                  <geom pos="-0.03 0.0065 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                  <geom size="0.004 0.02 0.002" pos="-0.03 0.0065 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0 0.046 -0.036" type="mesh" mesh="ankle" class="visual"/>
+                  <geom size="0.024 0.05 0.002" pos="0 0.046 -0.034" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.0105 0.002 0.025" pos="0 0.094 -0.011" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.0105 0.002 0.025" pos="0 -0.002 -0.011" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.024 0.002 0.004" pos="0 -0.002 -0.032" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.007 0.002 0.022128" pos="-0.0104 -0.002 -0.0093" quat="0.987173 0 0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.007 0.002 0.022128" pos="0.0104 -0.002 -0.0093" quat="0.987173 0 -0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.007 0.002 0.022128" pos="-0.0104 0.094 -0.0093" quat="0.987173 0 0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.007 0.002 0.022128" pos="0.0104 0.094 -0.0093" quat="0.987173 0 -0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.024 0.002 0.004" pos="0 0.094 -0.032" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0.06 0.1635 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                  <geom size="0.004 0.02 0.002" pos="0.06 0.1635 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="-0.03 0.1635 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                  <geom size="0.004 0.02 0.002" pos="-0.03 0.1635 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="-0.145 0.085 -0.0395" quat="0.707105 0 0 -0.707108" type="mesh" mesh="foot_printed_right" class="visual"/>
+                  <geom size="0.077 0.053 0.00175" pos="0.015 0.092 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.0095 0.039 0.00175" pos="0.015 0.0055 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.008 0.039 0.00175" pos="0.015 0.177 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.07 0.008 0.0035" pos="-0.03 0.085 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.0395 0.008 0.0035" pos="0.06 0.1155 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.01 0.022 0.0035" pos="0.046 0.025 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.01 0.022 0.0035" pos="0.046 0.086 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0.06 0.0065 -0.034" quat="1.32679e-06 0 0 1" type="mesh" mesh="load_cell" class="visual"/>
+                  <geom size="0.004 0.02 0.002" pos="0.06 0.0065 -0.032" quat="1.32679e-06 0 0 1" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0.0234568 0.0618337 -0.0463905" quat="0.707105 0 0 -0.707108" type="mesh" mesh="foot_cover" class="visual"/>
+                  <geom size="0.02 0.053 0.0095" pos="0.0149998 0.120225 -0.0192245" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.0255 0.037 0.001" pos="0.0149998 0.1625 -0.0232245" quat="0.171497 0.685994 0.685997 -0.171495" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="-0.03 -0.007 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                  <geom size="0.006 0.005" pos="-0.03 -0.007 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.006" pos="-0.03 -0.007 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="-0.03 0.177 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                  <geom size="0.006 0.005" pos="-0.03 0.177 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.006" pos="-0.03 0.177 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0.06 0.177 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                  <geom size="0.006 0.005" pos="0.06 0.177 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.006" pos="0.06 0.177 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom pos="0.06 -0.007 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                  <geom size="0.006 0.005" pos="0.06 -0.007 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                  <geom size="0.006" pos="0.06 -0.007 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="l_hip_1" pos="0 0.055 0" quat="0.707105 -0.707108 0 0">
+        <inertial pos="1.20984e-11 0.010315 -1.06799e-07" quat="-6.86297e-06 0.707107 -6.86297e-06 0.707107" mass="0.0689735" diaginertia="0.000110523 0.00010714 2.18955e-05" />
+      <joint name="LHipYaw" pos="0 0 0" axis="0 1 0" range="-1.5708 1.5708" class="mx106"/>
+        <geom pos="0 -0.0006 -0.018" type="mesh" mesh="hip_u_connector" class="visual"/>
+        <geom size="0.018 0.05 0.002" pos="0 0.0014 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.0130115 0.002 0.0265" pos="-0.048 0.0259 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.0130115 0.002 0.0265" pos="0.048 0.0259 0" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.025 0.002" pos="0 -0.0006 0.002" quat="0.499998 -0.5 0.5 0.500002" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <body name="l_hip_2" pos="-0.046 0.0414 0" quat="9.38184e-07 0.707105 9.38187e-07 -0.707108">
+        <inertial pos="-3.60783e-06 -0.0155281 -0.0486443" quat="0.995779 0.0129709 0.00328276 -0.0908052" mass="0.319807" diaginertia="0.000268692 0.000268184 0.000121228" />
+        <joint name="LHipRoll" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" class="mx106"/>
+        <geom pos="0.0145 0.007 -0.043" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="mesh" mesh="motor_connector" class="visual"/>
+        <geom size="0.014 0.003 0.00285" pos="0.0175 -0.007 -0.04585" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.014 0.00175 0.00295" pos="0.01875 -0.007 -0.05165" quat="9.38184e-07 -0.707105 0.707108 -9.38187e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom pos="0 0 -0.026" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-106_body" class="visual"/>
+        <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.026" quat="0.707105 -0.707108 0 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom pos="0 0 -0.0691" quat="0.499998 -0.5 0.5 0.500002" type="mesh" mesh="mx-106_body" class="visual"/>
+        <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.0691" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom pos="-0.0145 -0.021 -0.043" quat="9.38184e-07 0.707105 0.707108 9.38187e-07" type="mesh" mesh="motor_connector" class="visual"/>
+        <geom size="0.014 0.003 0.00285" pos="-0.0175 -0.007 -0.04585" quat="9.38184e-07 0.707105 0.707108 9.38187e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom size="0.014 0.00175 0.00295" pos="-0.01875 -0.007 -0.05165" quat="9.38184e-07 0.707105 0.707108 9.38187e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        <geom pos="0.0151 0.0105 -0.0892" quat="1.32679e-06 0 0 -1" type="mesh" mesh="thrustbearingholder" class="visual"/>
+          <body name="l_upper_leg" pos="0.026 0 -0.0691" quat="0.707105 0 -0.707108 0">
+          <inertial pos="-0.00463142 -0.132966 0.0263223" quat="0.711791 0.700947 -0.0442682 -0.00815018" mass="0.242311" diaginertia="0.000497319 0.000454544 0.000118674" />
+          <joint name="LHipPitch" pos="0 0 0" axis="0 0 -1" range="-1.91986 2.0944" class="mx106"/>
+          <geom pos="-0.0182355 -0.130164 0.0283472" quat="1.28159e-06 0.965926 0.258819 3.434e-07" type="mesh" mesh="knee_connector" class="visual"/>
+          <geom size="0.021 0.0089913 0.01225" pos="-0.0157399 -0.134486 0.01025" quat="1.28159e-06 0.965926 0.258819 3.434e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.0124063 0.002" pos="-0.0140324 -0.137444 3.76764e-08" quat="1.28159e-06 0.965926 0.258819 3.434e-07" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.018 0.001" pos="4.4347e-08 -3.96328e-07 0.05" quat="1.32679e-06 -1 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="-0.0178025 -0.129914 0.0206528" quat="0.258821 0 0 -0.965925" type="mesh" mesh="knee_connector" class="visual"/>
+          <geom size="0.021 0.0089913 0.01225" pos="-0.0153069 -0.134236 0.03875" quat="0.258821 0 0 -0.965925" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.0124063 0.002" pos="-0.0135994 -0.137194 0.049" quat="0.258821 0 0 -0.965925" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.00435596 -0.168793 0.0245" quat="0.258821 0 0 -0.965925" type="mesh" mesh="xh-540" class="visual"/>
+          <geom size="0.017 0.02925 0.022" pos="-0.00339404 -0.15537 0.0245" quat="0.258821 0 0 -0.965925" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="-0.014 -0.138 0.053" quat="0.258821 0 0 0.965925" type="mesh" mesh="spring_holder_upper" class="visual"/>
+          <geom size="0.014 0.011" pos="-0.014 -0.138 0.064" quat="0.258821 0 0 0.965925" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.018 0.001" pos="4.4347e-08 3.96328e-07 -0.001" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0 0 -0.002" quat="9.38184e-07 9.38187e-07 -0.707108 -0.707105" type="mesh" mesh="upper_leg" class="visual"/>
+          <geom size="0.018 0.001" pos="0 0 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.01 0.001" pos="-0.009329 -0.143 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001" pos="-0.0148403 -0.132659 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.019 0.001 0.0510495" pos="-0.00259365 -0.0830903 -0.003" quat="0.0922969 0.0922973 0.701059 0.701056" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.005 0.001 0.005" pos="-0.00161489 -0.0253454 -0.003" quat="0.701056 0.701059 -0.092296 -0.0922957" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.01 0.001" pos="-0.009329 -0.143 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.00400978 -0.00242195 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.00770728 -0.00682845 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0105834 -0.0118101 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0125508 -0.0172154 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0135497 -0.0228803 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0135497 -0.0286326 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0125508 -0.0342975 -0.003" quat="1.76027e-12 -1.32679e-06 1 -1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0 0 0.053" quat="9.38184e-07 9.38187e-07 0.707108 0.707105" type="mesh" mesh="upper_leg" class="visual"/>
+          <geom size="0.018 0.001" pos="0 0 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.01 0.001" pos="-0.009329 -0.143 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.02 0.001" pos="-0.0148403 -0.132659 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.019 0.001 0.0510495" pos="-0.00259365 -0.0830903 0.052" quat="0.0922969 0.0922973 0.701059 0.701056" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.005 0.001 0.005" pos="-0.00161489 -0.0253454 0.052" quat="0.701056 0.701059 -0.092296 -0.0922957" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.01 0.001" pos="-0.009329 -0.143 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.00400978 -0.00242195 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.00770728 -0.00682845 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0105834 -0.0118101 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0125508 -0.0172154 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0135497 -0.0228803 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0135497 -0.0286326 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.017 0.001" pos="0.0125508 -0.0342975 0.052" quat="1.76058e-12 1.32679e-06 1 1.32679e-06" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="-0.0220754 -0.0939778 -0.002" quat="0.991445 0 0 -0.130526" type="mesh" mesh="upper_leg_spacer" class="visual"/>
+          <geom size="0.017575 0.019 0.0265" pos="-0.00281079 -0.0836106 0.0245" quat="0.991445 0 0 -0.130526" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <body name="l_lower_leg" pos="0.00435596 -0.168793 0.049" quat="0.0922969 0.0922973 0.701059 0.701056">
+            <inertial pos="0.00142428 -0.0188696 -0.0617069" quat="0.996912 -0.0755929 0.0212087 0.00123263" mass="0.0880635" diaginertia="0.000285346 0.000242808 7.1906e-05" />
+            <joint name="LKnee" pos="0 0 0" axis="0 1 0" range="0 2.96706" class="xh-540" ref="-0.15"/>
+            <geom pos="0 0.0035 0.17" quat="1.32679e-06 1 0 0" type="mesh" mesh="springholder_new" class="visual"/>
+            <geom size="0.0165 0.0015" pos="0 0.024 0" quat="0.707105 -0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.0105 0.00815" pos="0 0.01435 0" quat="0.707105 -0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.00217889 -0.0033 -0.145095" quat="1.32679e-06 0 0 -1" type="mesh" mesh="spring_holder_lower" class="visual"/>
+            <geom size="0.007 0.00455 0.0155" pos="0.0124789 0.00805 -0.0426663" quat="1.32553e-06 -0.0436194 -5.78739e-08 -0.999048" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0 0.0035 0" quat="1.32679e-06 -1 0 0" type="mesh" mesh="springholder_bottom" class="visual"/>
+            <geom size="0.015 0.00135" pos="0 0.0035 -0.00135" quat="0.707105 -0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.02797 -0.031481 -0.15413" type="mesh" mesh="knee_spacer" class="visual"/>
+            <geom size="0.018 0.00075" pos="4.35337e-09 -0.0505 -2.49321e-07" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="-0.00951565 -0.0495 -0.03402" quat="0.477716 -0.477713 0.521335 0.521332" type="mesh" mesh="lower_leg_spacer" class="visual"/>
+            <geom size="0.0322425 0.01675 0.0255" pos="0.00253479 -0.024 -0.0569148" quat="0.477716 -0.477713 0.521335 0.521332" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0.02797 0.019019 -0.15413" type="mesh" mesh="knee_spacer" class="visual"/>
+            <geom size="0.018 0.00075" pos="4.35337e-09 -1.89754e-08 -2.49321e-07" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0 -0.0505 -0.17" type="mesh" mesh="lower_leg" class="visual"/>
+            <geom size="0.018 0.001" pos="0 -0.0515 -0.17" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.018 0.001" pos="0 -0.0515 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="-0.004 -0.0515 -0.165" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="0.004 -0.0515 -0.005" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001 0.08" pos="-9.2229e-05 -0.0515 -0.085786" quat="0.999048 0 0.0436194 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom pos="0 0.0035 -0.17" type="mesh" mesh="lower_leg" class="visual"/>
+            <geom size="0.018 0.001" pos="0 0.0025 -0.17" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.018 0.001" pos="0 0.0025 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="-0.004 0.0025 -0.165" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.02 0.001" pos="0.004 0.0025 -0.005" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <geom size="0.017 0.001 0.08" pos="-9.2229e-05 0.0025 -0.085786" quat="0.999048 0 0.0436194 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+            <body name="l_ankle" pos="0 -0.0505 -0.17" quat="9.38184e-07 -9.38187e-07 -0.707108 0.707105">
+              <inertial pos="0.0204593 -0.0155281 -0.0260139" quat="-0.0489913 0.702167 -0.0697898 0.706888" mass="0.319807" diaginertia="0.000272939 0.000268433 0.000116635" />
+              <joint name="LAnklePitch" pos="0 0 0" axis="0 0 -1" range="-1.22173 1.74533" class="mx106"/>
+              <geom pos="0.0261 0.007 -0.0405" quat="0.499998 0.5 -0.500002 -0.5" type="mesh" mesh="motor_connector" class="visual"/>
+              <geom size="0.014 0.003 0.00285" pos="0.02325 -0.007 -0.0435" quat="0.499998 0.5 -0.500002 -0.5" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+              <geom size="0.014 0.00175 0.00295" pos="0.01745 -0.007 -0.04475" quat="0.499998 0.5 -0.500002 -0.5" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+              <geom pos="-0.0201 0.0105 -0.04" quat="9.38184e-07 -0.707105 -9.38187e-07 -0.707108" type="mesh" mesh="thrustbearingholder" class="visual"/>
+              <geom pos="0.0261 -0.021 -0.0115" quat="0.499998 -0.5 -0.500002 0.5" type="mesh" mesh="motor_connector" class="visual"/>
+              <geom size="0.014 0.003 0.00285" pos="0.02325 -0.007 -0.0085" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+              <geom size="0.014 0.00175 0.00295" pos="0.01745 -0.007 -0.00725" quat="0.499998 -0.5 -0.500002 0.5" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+              <geom pos="0 0 -0.026" quat="0.707105 -0.707108 0 0" type="mesh" mesh="mx-106_body" class="visual"/>
+              <geom size="0.0201 0.023 0.03255" pos="0 -0.01805 -0.026" quat="0.707105 -0.707108 0 0" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+              <geom pos="0.0431 0 -0.026" quat="0.499998 -0.5 0.5 0.500002" type="mesh" mesh="mx-106_body" class="visual"/>
+              <geom size="0.0201 0.023 0.03255" pos="0.0431 -0.01805 -0.026" quat="0.499998 -0.5 0.5 0.500002" type="box" rgba="0.262745 0.282353 0.301961 1" class="collision"/>
+                <body name="l_foot_l" pos="0.0691 0 -0.026" quat="0.499998 0.5 -0.5 0.500002">
+                <inertial pos="-0.0121076 0.0909614 -0.0301459" quat="0.476206 0.512458 -0.497419 0.51302" mass="0.274879" diaginertia="0.00122374 0.000943172 0.000329929" />
+                <joint name="LAnkleRoll" pos="0 0 0" axis="0 -1 0" range="-1.0472 1.0472" class="mx106"/>
+                <site name="l_foot"/>
+                <geom pos="-0.06 0.1635 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                <geom size="0.004 0.02 0.002" pos="-0.06 0.1635 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="0.03 0.0065 -0.034" quat="1.32679e-06 0 0 1" type="mesh" mesh="load_cell" class="visual"/>
+                <geom size="0.004 0.02 0.002" pos="0.03 0.0065 -0.032" quat="1.32679e-06 0 0 1" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="-0.015 0.085 -0.0395" quat="0.707105 0 0 -0.707108" type="mesh" mesh="foot_printed_left" class="visual"/>
+                <geom size="0.077 0.053 0.00175" pos="-0.015 0.092 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.0095 0.039 0.00175" pos="-0.015 0.0055 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.008 0.039 0.00175" pos="-0.015 0.177 -0.03775" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.07 0.008 0.0035" pos="0.03 0.085 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.0395 0.008 0.0035" pos="-0.06 0.1155 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.01 0.022 0.0035" pos="-0.046 0.025 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.01 0.022 0.0035" pos="-0.046 0.086 -0.0325" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="-0.00654316 0.0618337 -0.0463905" quat="0.707105 0 0 -0.707108" type="mesh" mesh="foot_cover" class="visual"/>
+                <geom size="0.02 0.053 0.0095" pos="-0.0150002 0.120225 -0.0192245" quat="0.707105 0 0 -0.707108" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.0255 0.037 0.001" pos="-0.0150002 0.1625 -0.0232245" quat="0.171497 0.685994 0.685997 -0.171495" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="0 0.046 -0.036" type="mesh" mesh="ankle" class="visual"/>
+                <geom size="0.024 0.05 0.002" pos="0 0.046 -0.034" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.0105 0.002 0.025" pos="0 0.094 -0.011" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.0105 0.002 0.025" pos="0 -0.002 -0.011" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.024 0.002 0.004" pos="0 -0.002 -0.032" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.007 0.002 0.022128" pos="-0.0104 -0.002 -0.0093" quat="0.987173 0 0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.007 0.002 0.022128" pos="0.0104 -0.002 -0.0093" quat="0.987173 0 -0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.007 0.002 0.022128" pos="-0.0104 0.094 -0.0093" quat="0.987173 0 0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.007 0.002 0.022128" pos="0.0104 0.094 -0.0093" quat="0.987173 0 -0.159653 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.024 0.002 0.004" pos="0 0.094 -0.032" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="0.03 0.1635 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                <geom size="0.004 0.02 0.002" pos="0.03 0.1635 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="-0.06 0.0065 -0.034" quat="1 0 0 0" type="mesh" mesh="load_cell" class="visual"/>
+                <geom size="0.004 0.02 0.002" pos="-0.06 0.0065 -0.032" quat="1 0 0 0" type="box" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="-0.06 -0.007 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                <geom size="0.006 0.005" pos="-0.06 -0.007 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.006" pos="-0.06 -0.007 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="-0.06 0.177 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                <geom size="0.006 0.005" pos="-0.06 0.177 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.006" pos="-0.06 0.177 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="0.03 0.177 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                <geom size="0.006 0.005" pos="0.03 0.177 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.006" pos="0.03 0.177 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom pos="0.03 -0.007 -0.044" quat="0.707105 0 0 -0.707108" type="mesh" mesh="flex_stollen" class="visual"/>
+                <geom size="0.006 0.005" pos="0.03 -0.007 -0.039" quat="0.707105 0 0 -0.707108" type="cylinder" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                <geom size="0.006" pos="0.03 -0.007 -0.044" quat="0.707105 0 0 -0.707108" rgba="0.717647 0.717647 0.717647 1" class="collision"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="neck" pos="-0.0095 0 0.2345" quat="1.32679e-06 -1 0 0">
+        <inertial pos="0.0186912 -7.75177e-05 -0.0220143" quat="-0.0168892 0.773726 -5.81151e-05 0.633295" mass="0.148233" diaginertia="6.35428e-05 5.76425e-05 3.79234e-05"/>
+        <joint name="HeadPan" pos="0 0 0" axis="0 0 -1" range="-2.35619 2.35619" class="mx64"/> 
+        <geom pos="0.0035 0 -0.0093" type="mesh" mesh="connector_shoulder" class="visual"/>
+        <geom size="0.018 0.02 0.00465" pos="0.0035 0 -0.00465" type="box" rgba="0.247059 0.247059 0.247059 1" class="collision"/>
+        <geom pos="0.036 0 -0.024" quat="9.38184e-07 0.707105 -9.38187e-07 0.707108" type="mesh" mesh="mx-64-body" class="visual"/>
+        <geom size="0.0201 0.0205 0.03055" pos="0.01845 0 -0.024" quat="9.38184e-07 0.707105 -9.38187e-07 0.707108" type="box" rgba="0.247059 0.247059 0.247059 1" class="collision"/>
+        <body name="head" pos="0.036 0.0235 -0.024" quat="0.707105 -0.707108 0 0">
+          <inertial pos="0.000411708 0.0674937 -0.0200647" quat="0.637844 0.233437 -0.515718 0.522204" mass="0.301358" diaginertia="0.000445949 0.000352444 0.000301483" />
+          <joint name="HeadTilt" pos="0 0 0" axis="0 0 1" range="-1.5708 1.0472" class="mx64"/>
+          <!-- DEF camera_optical_frame Solid {
+            translation 0.043200 0.080000 -0.023500
+            rotation 0.7071068 0.0 0.7071068 3.1415927
+            children [
+              Camera {
+                rotation 0.5773503 -0.5773503 0.5773503 2.0943951
+                name "camera"
+                fieldOfView IS cameraFOV
+                width IS cameraWidth
+                height IS cameraHeight
+              }
+              DEF camera_frame Transform {}
+            ]
+            name "camera_optical_frame"
+          } 
+            field  SFFloat     cameraFOV       1.517 rad - 86.917698 degrees
+            field  SFInt32     cameraWidth     800
+            field  SFInt32     cameraHeight    600
+
+            verticalFOV = 2 * atan(tan(horizontalFOV * 0.5) * (height / width)) = 1.23574275 rad - 70,80284413907 degrees
+          -->
+          <camera name="head_camera" pos="0.043200 0.080000 -0.023500" quat="1 0 -1 0" mode="fixed" resolution="800 600" fovy="70.802" />
+          <geom pos="0.0432 0.0795 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="lense" class="visual"/>
+          <geom size="0.0215 0.0081" pos="0.0351 0.0795 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.01675 0.006" pos="0.021 0.0795 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom type="mesh" mesh="sea_ninjaflex" class="visual"/>
+          <geom size="0.025 0.0045" pos="0 0 0.0045" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0 0 0.009" quat="1.32679e-06 0 0 1" type="mesh" mesh="sea_connector" class="visual"/>
+          <geom size="0.025 0.0025" pos="0 0 0.0115" quat="1.32679e-06 0 0 1" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.01 0.065 0.024" quat="1 0 0 0" type="mesh" mesh="camera_side_basler_wolfgang_v2.2_right" class="visual"/>
+          <geom size="0.0635 0.0275 0.00355051" pos="0.0007814 0.0775 0.00255051" quat="1 0 0 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.01 0.065 -0.051" quat="1 0 0 0" type="mesh" mesh="camera_side_basler_wolfgang_v2.2_left" class="visual"/>
+          <geom size="0.0635 0.0275 0.00355051" pos="0.0007814 0.0775 -0.0495505" quat="1 0 0 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.015 0.0795 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="mesh" mesh="basler_ace_gige_c-mount_v01" class="visual"/>
+          <geom size="0.0145 0.021 0.0145" pos="-0.018 0.0795 -0.0235" quat="0.499998 -0.5 -0.5 -0.500002" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.014 0.006" pos="0.009 0.0795 -0.0235" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom pos="0.003 0.065 -0.037945" quat="0.707105 0 -0.707108 0" type="mesh" mesh="camera_lower_basler_wolfgang_imu_v2.2" class="visual"/>
+          <geom size="0.003 0.036 0.015" pos="0 0.029 -0.0505" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.003 0.036 0.015" pos="0 0.029 0.0165" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.0365 0.0035 0.015" pos="0 0.0615 -0.017" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+          <geom size="0.0225 0.011 0.0275" pos="-0.0125 0.069 -0.0235" quat="0.707105 0 -0.707108 0" type="box" rgba="0.647059 0.647059 0.647059 1" class="collision"/>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+  <position joint="RShoulderPitch" name="RShoulderPitch" class="mx106"/>
+  <position joint="RShoulderRoll" name="RShoulderRoll" class="mx106"/>
+  <position joint="RElbow" name="RElbow" class="mx64"/>
+  <position joint="LShoulderPitch" name="LShoulderPitch" class="mx106"/>
+  <position joint="LShoulderRoll" name="LShoulderRoll" class="mx106"/>
+  <position joint="LElbow" name="LElbow" class="mx64"/>
+  <position joint="RHipYaw" name="RHipYaw" class="mx106"/>
+  <position joint="RHipRoll" name="RHipRoll" class="mx106"/>
+  <position joint="RHipPitch" name="RHipPitch" class="mx106"/>
+  <position joint="RKnee" name="RKnee" class="xh-540"/>
+  <position joint="RAnklePitch" name="RAnklePitch" class="mx106"/>
+  <position joint="RAnkleRoll" name="RAnkleRoll" class="mx106"/>
+  <position joint="LHipYaw" name="LHipYaw" class="mx106"/>
+  <position joint="LHipRoll" name="LHipRoll" class="mx106"/>
+  <position joint="LHipPitch" name="LHipPitch" class="mx106"/>
+  <position joint="LKnee" name="LKnee" class="xh-540"/>
+  <position joint="LAnklePitch" name="LAnklePitch" class="mx106"/>
+  <position joint="LAnkleRoll" name="LAnkleRoll" class="mx106"/>
+  <position joint="HeadPan" name="HeadPan" class="mx64"/>
+  <position joint="HeadTilt" name="HeadTilt" class="mx64"/>
+  </actuator>
+
+  <sensor>
+    <gyro site="imu" name="gyro"/>
+    <velocimeter site="imu" name="local_linvel"/>
+    <accelerometer site="imu" name="accelerometer"/>
+    <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"/>
+    <framepos objtype="site" objname="imu" name="position"/>
+    <framequat objtype="site" objname="imu" name="orientation"/>
+    <framelinvel objtype="site" objname="l_foot" name="l_foot_global_linvel"/>
+    <framelinvel objtype="site" objname="r_foot" name="r_foot_global_linvel"/>
+    <framexaxis objtype="site" objname="l_foot" name="l_foot_upvector"/>
+    <framexaxis objtype="site" objname="r_foot" name="r_foot_upvector"/>
+    <framepos objtype="site" objname="l_foot" name="l_foot_pos"/>
+    <framepos objtype="site" objname="r_foot" name="r_foot_pos"/>
+    <!-- <contact name="l_foot1_floor_found" geom1="l_foot1" geom2="floor" reduce="mindist" num="1" data="found"/>
+    <contact name="r_foot1_floor_found" geom1="r_foot1" geom2="floor" reduce="mindist" num="1" data="found"/>
+    <contact name="right_foot_left_foot_found" geom1="l_foot1" geom2="r_foot1" reduce="mindist" num="1" data="found"/> -->
+  </sensor>
+</mujoco>