diff --git a/sim/macro.py b/sim/macro.py
new file mode 100644
index 00000000..3e2cac05
--- /dev/null
+++ b/sim/macro.py
@@ -0,0 +1,236 @@
+"""
+python sim/macro.py  --embodiment stompymicro
+"""
+import argparse
+import math
+import os
+from collections import deque
+from copy import deepcopy
+
+import mujoco
+import mujoco_viewer
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from tqdm import tqdm
+
+from sim.scripts.create_mjcf import load_embodiment
+
+
+class Sim2simCfg:
+    def __init__(
+        self,
+        embodiment,
+        frame_stack=15,
+        c_frame_stack=3,
+        sim_duration=60.0,
+        dt=0.001,
+        decimation=10,
+        cycle_time=0.4,
+        tau_factor=3,
+        lin_vel=2.0,
+        ang_vel=1.0,
+        dof_pos=1.0,
+        dof_vel=0.05,
+        clip_observations=18.0,
+        clip_actions=18.0,
+        action_scale=0.25,
+    ):
+        self.robot = load_embodiment(embodiment)
+
+        self.num_actions = len(self.robot.all_joints())
+        self.frame_stack = frame_stack
+        self.c_frame_stack = c_frame_stack
+        self.num_single_obs = 11 + self.num_actions * self.c_frame_stack
+        self.num_observations = int(self.frame_stack * self.num_single_obs)
+
+        self.sim_duration = sim_duration
+        self.dt = dt
+        self.decimation = decimation
+
+        self.cycle_time = cycle_time
+        self.kps = np.array(list(self.robot.stiffness().values()) + list(self.robot.stiffness().values()))
+        self.kds = np.array(list(self.robot.damping().values()) + list(self.robot.damping().values()))
+
+        self.lin_vel = lin_vel
+        self.ang_vel = ang_vel
+        self.dof_pos = dof_pos
+        self.dof_vel = dof_vel
+
+        self.clip_observations = clip_observations
+        self.clip_actions = clip_actions
+
+        self.action_scale = action_scale
+
+
+def pd_control(target_q, q, kp, target_dq, dq, kd, default):
+    """Calculates torques from position commands"""
+    return kp * (target_q + default - q) - kd * dq
+
+
+def get_scripted_joint_targets(current_time, cfg, joints):
+    """
+    Returns the target joint positions for the robot at the given time.
+
+    Args:
+        current_time (float): The current simulation time.
+        cfg (Sim2simCfg): The simulation configuration.
+
+    Returns:
+        np.ndarray: The target joint positions.
+    """
+    global save_q
+
+    # Initialize target positions
+    target_q = np.zeros(cfg.num_actions, dtype=np.double)
+
+    # Define time intervals for different actions
+    # You can adjust these intervals as needed
+    t0 = .033  # Time to let it fall
+    t1 = 1.0  # Time to move arms to position
+    t2 = 2.0  # Time to bend knees
+    t3 = 3.0  # Time to stand up
+    t4 = 4.0  # Time to bend knees
+    # lie down
+    # Lie down to arms up (0 to t1)
+    # breakpoint()
+    # let it fall
+    # <key qpos='0 0 0.32 0.984816 0.173603 0 0 -1.51815 0.401126 -0.01571 1.4309 -0.4363 0 -1.571 0 0.463386 1.571 1.571 1.571 0 -0.463386 -1.571 -1.571'/>
+    # 
+    if current_time <= t0:
+        pass
+    # else:
+    elif current_time <= t1:
+        progress_1 = (current_time - t0) / (t1 - t0)
+        # Interpolate arm joints from initial to target position
+        # Assuming arm joints are indices 0 and 1 (adjust based on your robot)
+        arm_joint_indices = [joints["right_shoulder_pitch"], joints["left_shoulder_pitch"]]  # Replace with actual indices
+        arm_target_positions = [-1.55, 1.55] # Raise arms up
+
+        for idx, joint in enumerate(arm_joint_indices):
+            target_q[joint] = np.interp(progress_1, [0, 1], [0, arm_target_positions[idx]])
+
+        save_q = target_q
+    elif current_time <= t2:
+    # else:
+        target_q = save_q
+        progress_2 = (current_time - t1) / (t2 - t1)
+        # Interpolate arm joints from initial to target position
+        # Assuming arm joints are indices 0 and 1 (adjust based on your robot)
+        arm_joint_indices = [joints["right_shoulder_yaw"], joints["left_shoulder_yaw"]]  # Replace with actual indices
+        arm_target_positions = [0.436, -0.436, ]  # Raise arms up
+
+        for idx, joint in enumerate(arm_joint_indices):
+            target_q[joint] = np.interp(progress_2, [0, 1], [0, arm_target_positions[idx]])
+
+        save_q = target_q
+    # Arms up to bend knees (t1 to t2)
+    elif current_time <= t3:
+        target_q = save_q
+        progress = (current_time - t1) / (t2 - t1)
+
+        knee_joint_indices = [joints["right_knee_pitch"], joints["left_knee_pitch"]]  # Replace with actual indices
+        knee_bend_positions = np.deg2rad([-90, 90])  # Bend knees
+        for idx, joint in enumerate(knee_joint_indices):
+            target_q[joint] = np.interp(progress, [0, 1], [0, knee_bend_positions[idx]])
+        save_q = target_q
+    # Bend knees to stand up (t2 to t3)
+
+    else:
+        target_q = save_q
+        progress = (current_time - t3) / (t4 - t3)
+        # Knees extend to standing position
+        pitch_joint_indices = [joints["right_hip_pitch"], joints["left_hip_pitch"]]  # Replace with actual indices
+        pitch_positions = np.deg2rad([-90, 90])  # Bend knees
+        for idx, joint in enumerate(pitch_joint_indices):
+            target_q[joint] = np.interp(progress, [0, 1], [0, pitch_positions[idx]])
+        save_q = target_q
+
+    return target_q
+
+def run_mujoco_scripted(cfg):
+    """
+    Run the Mujoco simulation using scripted joint positions.
+
+    Args:
+        cfg: The configuration object containing simulation settings.
+
+    Returns:
+        None
+    """
+    model_dir = os.environ.get("MODEL_DIR")
+    mujoco_model_path = f"{model_dir}/{args.embodiment}/robot_fixed.xml"
+
+    model = mujoco.MjModel.from_xml_path(mujoco_model_path)
+    model.opt.timestep = cfg.dt
+    data = mujoco.MjData(model)
+
+    # Initialize default positions
+    try:
+        data.qpos = model.keyframe("default").qpos
+        default = deepcopy(model.keyframe("default").qpos)[-cfg.num_actions:]
+        print("Default position:", default)
+    except:
+        print("No default position found, using zero initialization")
+        default = np.zeros(cfg.num_actions)
+
+    mujoco.mj_step(model, data)
+    data.qvel = np.zeros_like(data.qvel)
+    data.qacc = np.zeros_like(data.qacc)
+    viewer = mujoco_viewer.MujocoViewer(model, data)
+
+    mujoco.mj_step(model, data)
+    joints = {}
+    for ii in range(1, len(data.ctrl)):
+        joints[data.joint(ii).name] = data.joint(ii).id - 1
+
+    # Initialize target joint positions
+    target_q = np.zeros((cfg.num_actions), dtype=np.double)
+
+    # Simulation loop
+    sim_steps = int(cfg.sim_duration / cfg.dt)
+    for step in tqdm(range(sim_steps), desc="Simulating..."):
+        current_time = step * cfg.dt
+        q = data.qpos[-cfg.num_actions:].astype(np.double)
+        dq = data.qvel[-cfg.num_actions:].astype(np.double)
+
+        # Update target_q based on scripted behavior
+        target_q = get_scripted_joint_targets(current_time, cfg, joints)
+        target_dq = np.zeros((cfg.num_actions), dtype=np.double)
+        tau = pd_control(target_q, q, cfg.kps, target_dq, dq, cfg.kds, default)
+        
+        print("tau:", tau)
+        print("q:", q)
+        print("target_q:", target_q)
+        data.ctrl = tau
+
+        mujoco.mj_step(model, data)
+        viewer.render()
+
+    viewer.close()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Deployment script.")
+    parser.add_argument("--embodiment", type=str, required=True, help="embodiment")
+    args = parser.parse_args()
+
+    if args.embodiment == "stompypro":
+        cfg = Sim2simCfg(
+            args.embodiment,
+            sim_duration=60.0,
+            dt=0.001,
+            decimation=10,
+            cycle_time=0.4,
+            tau_factor=3.0,
+        )
+    elif args.embodiment == "stompymicro":
+        cfg = Sim2simCfg(
+            args.embodiment,
+            sim_duration=60.0,
+            dt=0.001,
+            decimation=10,
+            cycle_time=0.4,
+            tau_factor=10.,
+        )
+
+    run_mujoco_scripted(cfg)
diff --git a/sim/resources/stompymicro/joints.py b/sim/resources/stompymicro/joints.py
index 78156914..4ac62cf2 100644
--- a/sim/resources/stompymicro/joints.py
+++ b/sim/resources/stompymicro/joints.py
@@ -55,13 +55,13 @@ def __str__(self) -> str:
 class LeftArm(Node):
     shoulder_yaw = "left_shoulder_yaw"
     shoulder_pitch = "left_shoulder_pitch"
-    elbow_pitch = "left_elbow_yaw"  # FIXME: yaw vs pitch
+    elbow_yaw = "left_elbow_yaw"  # FIXME: yaw vs pitch
 
 
 class RightArm(Node):
     shoulder_yaw = "right_shoulder_yaw"
     shoulder_pitch = "right_shoulder_pitch"
-    elbow_pitch = "right_elbow_yaw"  # FIXME: yaw vs pitch
+    elbow_yaw = "right_elbow_yaw"  # FIXME: yaw vs pitch
 
 
 class LeftLeg(Node):
@@ -89,63 +89,63 @@ class Robot(Node):
     height = 0.293
     rotation = [0, 0, 0.707, 0.707]
 
-    # left_arm = LeftArm()
-    # right_arm = RightArm()
+    left_arm = LeftArm()
+    right_arm = RightArm()
     legs = Legs()
 
     @classmethod
     def default_standing(cls) -> Dict[str, float]:
         return {
             # Legs
-            cls.legs.left.hip_pitch: 0.23,
+            cls.legs.left.hip_pitch: 0.0,
             cls.legs.left.hip_yaw: 0,
             cls.legs.left.hip_roll: 0,
-            cls.legs.left.knee_pitch: 0.741,
-            cls.legs.left.ankle_pitch: -0.5,
-            cls.legs.right.hip_pitch: -0.23,
+            cls.legs.left.knee_pitch: 0.0,
+            cls.legs.left.ankle_pitch: -0.0,
+            cls.legs.right.hip_pitch: -0.0,
             cls.legs.right.hip_yaw: 0,
             cls.legs.right.hip_roll: 0,
-            cls.legs.right.knee_pitch: -0.741,
-            cls.legs.right.ankle_pitch: 0.5,
+            cls.legs.right.knee_pitch: 0.0,
+            cls.legs.right.ankle_pitch: 0.0,
             # TODO: fixing this for debugging
             # Arms
-            # cls.left_arm.shoulder_pitch: 0,
-            # cls.left_arm.shoulder_yaw: 0,
-            # cls.left_arm.elbow_pitch: 0,
-            # cls.right_arm.shoulder_pitch: 0,
-            # cls.right_arm.shoulder_yaw: 0,
-            # cls.right_arm.elbow_pitch: 0,
+            cls.left_arm.shoulder_pitch: 0,
+            cls.left_arm.shoulder_yaw: 0,
+            cls.left_arm.elbow_yaw: 0,
+            cls.right_arm.shoulder_pitch: 0,
+            cls.right_arm.shoulder_yaw: 0,
+            cls.right_arm.elbow_yaw: 0,
         }
 
     @classmethod
     def default_limits(cls) -> Dict[str, Dict[str, float]]:
         return {
-            # # left arm
-            # Robot.left_arm.shoulder_pitch: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
-            # Robot.left_arm.shoulder_yaw: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
-            # Robot.left_arm.elbow_pitch: {
-            #     "lower": -1.2217305,
-            #     "upper": 1.2217305,
-            # },
-            # # right arm
-            # Robot.right_arm.shoulder_pitch: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
-            # Robot.right_arm.shoulder_yaw: {
-            #     "lower": -1.5707963,
-            #     "upper": 1.5707963,
-            # },
-            # Robot.right_arm.elbow_pitch: {
-            #     "lower": -1.2217305,
-            #     "upper": 1.2217305,
-            # },
+            # left arm
+            Robot.left_arm.shoulder_pitch: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
+            Robot.left_arm.shoulder_yaw: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
+            Robot.left_arm.elbow_yaw: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
+            # right arm
+            Robot.right_arm.shoulder_pitch: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
+            Robot.right_arm.shoulder_yaw: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
+            Robot.right_arm.elbow_yaw: {
+                "lower": -1.5707963,
+                "upper": 1.5707963,
+            },
             # left leg
             Robot.legs.left.hip_pitch: {
                 "lower": -1.5707963,
@@ -194,16 +194,14 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
     @classmethod
     def stiffness(cls) -> Dict[str, float]:
         return {
-            "hip_pitch": 17.681462808698132,
-            "hip_yaw": 17.681462808698132,
-            "hip_roll": 17.681462808698132,
-            "knee_pitch": 17.681462808698132,
-            "ankle_pitch": 17.681462808698132,
-            # "shoulder_pitch": 5,
-            # "shoulder_yaw": 5,
-            # "shoulder_roll": 5,
-            # "elbow_pitch": 5,
-            # "elbow_yaw": 5,
+            "hip_pitch": 5.5,
+            "hip_yaw": 5.5,
+            "hip_roll": 5.5,
+            "knee_pitch": 5.5,
+            "ankle_pitch": 5.5,
+            "shoulder_pitch": 5.5,
+            "shoulder_yaw": 5.5,
+            "elbow_yaw": 5.5,
         }
 
     # d_gains
@@ -215,11 +213,9 @@ def damping(cls) -> Dict[str, float]:
             "hip_roll": 0.5354656169048285,
             "knee_pitch": 0.5354656169048285,
             "ankle_pitch": 0.5354656169048285,
-            # "shoulder_pitch": 0.3,
-            # "shoulder_yaw": 0.3,
-            # "shoulder_roll": 0.3,
-            # "elbow_pitch": 0.3,
-            # "elbow_yaw": 0.3,
+            "shoulder_pitch": 0.5354656169048285,
+            "shoulder_yaw": 0.5354656169048285,
+            "elbow_yaw": 0.5354656169048285,
         }
 
     # pos_limits
@@ -231,11 +227,9 @@ def effort(cls) -> Dict[str, float]:
             "hip_roll": 1,
             "knee_pitch": 1,
             "ankle_pitch": 1,
-            # "shoulder_pitch": 1,
-            # "shoulder_yaw": 1,
-            # "shoulder_roll": 1,
-            # "elbow_pitch": 1,
-            # "elbow_yaw": 1,
+            "shoulder_pitch": 1,
+            "shoulder_yaw": 1,
+            "elbow_yaw": 1,
         }
 
     # vel_limits
@@ -247,11 +241,9 @@ def velocity(cls) -> Dict[str, float]:
             "hip_roll": 10,
             "knee_pitch": 10,
             "ankle_pitch": 10,
-            # "shoulder_pitch": 10,
-            # "shoulder_yaw": 10,
-            # "shoulder_roll": 10,
-            # "elbow_pitch": 10,
-            # "elbow_yaw": 10,
+            "shoulder_pitch": 10,
+            "shoulder_yaw": 10,
+            "elbow_yaw": 10,
         }
 
     @classmethod
@@ -263,9 +255,7 @@ def friction(cls) -> Dict[str, float]:
             "hip_roll": 0.05,
             "knee_pitch": 0.05,
             "ankle_pitch": 0.05,
-            # "ankle_pitch": 0.05,
-            # "elbow_yaw": 0.05,
-            # "elbow_pitch": 0.05,
+            "elbow_yaw": 0.05,
         }
 
 
diff --git a/sim/resources/stompymicro/robot.xml b/sim/resources/stompymicro/robot.xml
new file mode 100644
index 00000000..4f9e8a25
--- /dev/null
+++ b/sim/resources/stompymicro/robot.xml
@@ -0,0 +1,107 @@
+<mujoco model="opus">
+  <compiler angle="radian"/>
+
+  <asset>
+    <mesh name="Torso" file="Torso.stl"/>
+    <mesh name="shoulder_yaw_right" file="shoulder_yaw_right.stl"/>
+    <mesh name="shoulder_yaw_left" file="shoulder_yaw_left.stl"/>
+    <mesh name="right_shoulder_yaw_motor" file="right_shoulder_yaw_motor.stl"/>
+    <mesh name="hip_yaw_right" file="hip_yaw_right.stl"/>
+    <mesh name="hip_yaw_left" file="hip_yaw_left.stl"/>
+    <mesh name="left_shoulder_yaw_motor" file="left_shoulder_yaw_motor.stl"/>
+    <mesh name="hip_roll_right" file="hip_roll_right.stl"/>
+    <mesh name="hip_roll_left" file="hip_roll_left.stl"/>
+    <mesh name="Left_Hand" file="Left_Hand.stl"/>
+    <mesh name="hand_right" file="hand_right.stl"/>
+    <mesh name="knee_pitch_right" file="knee_pitch_right.stl"/>
+    <mesh name="knee_pitch_right_2" file="knee_pitch_right_2.stl"/>
+    <mesh name="ankle_pitch_right" file="ankle_pitch_right.stl"/>
+    <mesh name="ankle_pitch_left" file="ankle_pitch_left.stl"/>
+    <mesh name="foot_right" file="foot_right.stl"/>
+    <mesh name="foot_left" file="foot_left.stl"/>
+  </asset>
+
+  <worldbody>
+    <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Torso"/>
+    <body name="shoulder_yaw_right" pos="-0.0781365 -0.000335556 0.0430533" quat="0.5 0.5 0.5 0.5">
+      <inertial pos="-0.00037601 -0.00365665 -0.0174329" quat="0.647359 0.284476 0.647359 0.284476" mass="0.0403627" diaginertia="2.39887e-05 2.03034e-05 1.02268e-05"/>
+      <joint name="right_shoulder_pitch" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_right"/>
+      <body name="right_shoulder_yaw_motor" pos="-0.018825 -0.0162992 -0.03" quat="-1.64085e-08 0.707107 1.64085e-08 0.707107">
+        <inertial pos="-0.0149548 0.0320706 0.0181015" quat="0.688803 0.691851 0.153461 0.152782" mass="0.17146" diaginertia="0.000152631 0.000138758 4.07147e-05"/>
+        <joint name="right_shoulder_yaw" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+        <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_shoulder_yaw_motor"/>
+        <body name="Left_Hand" pos="-0.0300031 0.0643419 0.000250001" quat="0.976296 4.53101e-08 0 0.21644">
+          <inertial pos="0.0011185 0.0461324 0.0182544" quat="0.706815 0.706815 -0.0202995 -0.0202995" mass="0.0751849" diaginertia="5.67243e-05 4.71032e-05 1.47264e-05"/>
+          <joint name="right_elbow_yaw" pos="0 0 0" axis="0 0 1" range="-1.22173 1.22173" actuatorfrcrange="-1 1"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Left_Hand"/>
+        </body>
+      </body>
+    </body>
+    <body name="shoulder_yaw_left" pos="0.0781635 -0.000335556 0.0430533" quat="0.5 -0.5 -0.5 0.5">
+      <inertial pos="-0.000375909 0.00365652 -0.0174327" quat="0.284475 0.647359 0.284475 0.647359" mass="0.0403631" diaginertia="2.39889e-05 2.03034e-05 1.0227e-05"/>
+      <joint name="left_shoulder_pitch" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_left"/>
+      <body name="left_shoulder_yaw_motor" pos="-0.018825 0.0162992 -0.03" quat="-1.64085e-08 0.707107 -1.64085e-08 -0.707107">
+        <inertial pos="0.0149548 -0.0320706 -0.0181015" quat="0.689017 0.692092 0.152399 0.151787" mass="0.17146" diaginertia="0.000152247 0.000138758 4.10983e-05"/>
+        <joint name="left_shoulder_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+        <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_shoulder_yaw_motor"/>
+        <body name="hand_right" pos="0.0300031 -0.0643419 -0.000250001" quat="2.26551e-08 -0.976296 -0.21644 5.0225e-09">
+          <inertial pos="-0.00111857 0.0461326 0.0182546" quat="0.706815 0.706815 0.0202982 0.0202982" mass="0.0751837" diaginertia="5.67234e-05 4.71029e-05 1.47257e-05"/>
+          <joint name="left_elbow_yaw" pos="0 0 0" axis="0 0 1" range="-1.22173 1.22173" actuatorfrcrange="-1 1"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hand_right"/>
+        </body>
+      </body>
+    </body>
+    <body name="hip_yaw_right" pos="-0.0456365 -0.000335555 -0.0969467" quat="0.5 -0.5 -0.5 0.5">
+      <inertial pos="-0.00777521 -0.0101889 0.0152725" quat="0.889874 0.455956 0.00767715 -0.0129746" mass="0.103196" diaginertia="4.3188e-05 4.07436e-05 3.38931e-05"/>
+      <joint name="right_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_right"/>
+      <body name="hip_roll_right" pos="-0.0255 0.0025 0.01936" quat="0.707107 3.2817e-08 0.707107 -3.2817e-08">
+        <inertial pos="5.22112e-05 0.0427183 0.0279625" quat="0.585477 0.810687 0.0016716 0.000214999" mass="0.111293" diaginertia="6.5719e-05 4.6539e-05 3.49745e-05"/>
+        <joint name="right_hip_yaw" pos="0 0 0" axis="0 0 1" range="-0.0872665 1.5708" actuatorfrcrange="-1 1"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_right"/>
+        <body name="knee_pitch_right" pos="0 0.06776 0.015" quat="0.707107 0.707107 0 0">
+          <inertial pos="0.000718952 -0.00237466 -0.0219131" quat="0.702278 0.0508037 -0.0647173 0.707132" mass="0.110612" diaginertia="6.96382e-05 6.25773e-05 2.61051e-05"/>
+          <joint name="right_hip_roll" pos="0 0 0" axis="0 0 1" range="-0.785398 0.785398" actuatorfrcrange="-1 1"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right"/>
+          <body name="ankle_pitch_right" pos="0.018825 -9.39161e-10 -0.04221" quat="0.5 -0.5 -0.5 0.5">
+            <inertial pos="-5.29027e-07 0.0409224 0.0178642" quat="0.709959 0.704243 3.3304e-05 3.30359e-05" mass="0.109625" diaginertia="5.75137e-05 4.60143e-05 2.78632e-05"/>
+            <joint name="right_knee_pitch" pos="0 0 0" axis="0 0 1" range="-1.0472 1.0472" actuatorfrcrange="-1 1"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_right"/>
+            <body name="foot_right" pos="0 0.06022 -1.3e-08" quat="2.32051e-08 -1 0 0">
+              <inertial pos="-0.00778227 -0.0161914 -0.0178319" quat="0.488777 0.488777 0.510977 0.510977" mass="0.0406222" diaginertia="3.78658e-05 2.8894e-05 1.35969e-05"/>
+              <joint name="right_ankle_pitch" pos="0 0 0" axis="0 0 -1" range="-1.0472 1.0472" actuatorfrcrange="-1 1"/>
+              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_right"/>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+    <body name="hip_yaw_left" pos="0.0456635 -0.000335555 -0.0969467" quat="0.5 0.5 0.5 0.5">
+      <inertial pos="-0.00777616 0.0101887 0.0152718" quat="0.45592 0.889889 0.0131744 -0.00779523" mass="0.103202" diaginertia="4.31891e-05 4.07457e-05 3.38951e-05"/>
+      <joint name="left_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_left"/>
+      <body name="hip_roll_left" pos="-0.0255 -0.0025 0.01936" quat="1.64085e-08 -0.707107 -1.64085e-08 -0.707107">
+        <inertial pos="-5.22128e-05 0.0427184 0.0279626" quat="0.585474 0.810689 -0.00167161 -0.000214986" mass="0.111294" diaginertia="6.57191e-05 4.65391e-05 3.49746e-05"/>
+        <joint name="left_hip_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 0.0872665" actuatorfrcrange="-1 1"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_left"/>
+        <body name="knee_pitch_right_2" pos="0 0.06776 0.015" quat="0.707107 0.707107 0 0">
+          <inertial pos="-0.000643821 -0.00237466 -0.0244388" quat="0.706931 0.0631453 -0.0485932 0.70278" mass="0.110612" diaginertia="7.52582e-05 6.81301e-05 2.60379e-05"/>
+          <joint name="left_hip_roll" pos="0 0 0" axis="0 0 1" range="-0.785398 0.785398" actuatorfrcrange="-1 1"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right_2"/>
+          <body name="ankle_pitch_left" pos="-0.018775 9.39161e-10 -0.04596" quat="0.5 -0.5 -0.5 0.5">
+            <inertial pos="2.15197e-06 0.0383739 -0.019579" quat="0.737365 0.675494 4.40773e-06 4.03788e-06" mass="0.109625" diaginertia="5.33242e-05 4.18066e-05 2.78814e-05"/>
+            <joint name="left_knee_pitch" pos="0 0 0" axis="0 0 -1" range="-1.0472 1.0472" actuatorfrcrange="-1 1"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_left"/>
+            <body name="foot_left" pos="1.3459e-09 0.05647 -0.000450004" quat="2.32051e-08 -1 4.64102e-08 0">
+              <inertial pos="-0.00778227 -0.0161914 0.0178319" quat="0.488777 0.488777 0.510977 0.510977" mass="0.0406222" diaginertia="3.78658e-05 2.8894e-05 1.35969e-05"/>
+              <joint name="left_ankle_pitch" pos="0 0 0" axis="0 0 1" range="-1.0472 1.0472" actuatorfrcrange="-1 1"/>
+              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_left"/>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/sim/resources/stompymicro/robot_calibration.xml b/sim/resources/stompymicro/robot_calibration.xml
deleted file mode 100644
index 199afdbf..00000000
--- a/sim/resources/stompymicro/robot_calibration.xml
+++ /dev/null
@@ -1,181 +0,0 @@
-<mujoco model="torso">
-  <compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true"/>
-  <default>
-    <joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01"/>
-    <geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2"/>
-    <motor ctrllimited="true"/>
-    <equality solref="0.001 2"/>
-    <default class="visualgeom">
-      <geom material="visualgeom" condim="1" contype="0" conaffinity="0"/>
-    </default>
-  </default>
-  <option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81"/>
-  <asset>
-    <mesh name="Base_Motor_Mount_List_cGDi0X0HPtm1lf=Default" file="Base_Motor_Mount_List_cGDi0X0HPtm1lf=Default.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE" file="DRIVING_ROTOR_PLATE.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_2" file="DRIVING_ROTOR_PLATE_2.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_3" file="DRIVING_ROTOR_PLATE_3.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_4" file="DRIVING_ROTOR_PLATE_4.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_5" file="DRIVING_ROTOR_PLATE_5.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_6" file="DRIVING_ROTOR_PLATE_6.stl"/>
-    <mesh name="LX-16A" file="LX-16A.stl"/>
-    <mesh name="LX-16A_2" file="LX-16A_2.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_7" file="DRIVING_ROTOR_PLATE_7.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_8" file="DRIVING_ROTOR_PLATE_8.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_9" file="DRIVING_ROTOR_PLATE_9.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_10" file="DRIVING_ROTOR_PLATE_10.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_11" file="DRIVING_ROTOR_PLATE_11.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_12" file="DRIVING_ROTOR_PLATE_12.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_13" file="DRIVING_ROTOR_PLATE_13.stl"/>
-    <mesh name="DRIVING_ROTOR_PLATE_14" file="DRIVING_ROTOR_PLATE_14.stl"/>
-    <texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="108"/>
-    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true"/>
-    <material name="visualgeom" rgba="0.5 0.9 0.2 1"/>
-
-  </asset>
-
-  <worldbody>
-    <geom name="ground" type="plane" pos="0.001 0 0" quat="1 0 0 0" material="matplane" condim="1" conaffinity="15" size="0 0 1"/>
-    <light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
-    <light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
-
-    <body name="torso" pos="0 0 0.2" quat="0.0 0.0 0 1">
-      <!-- <joint type="free" name="root_joint"/> -->
-
-      <geom type="mesh" mesh="Base_Motor_Mount_List_cGDi0X0HPtm1lf=Default" class="visualgeom"/>
-      <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="Base_Motor_Mount_List_cGDi0X0HPtm1lf=Default" contype="0" conaffinity="0" group="1" density="0"/>
-      <geom pos="0.0713138 -0.0234828 0.108127" quat="0 -0.707107 0 -0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_3" class="visualgeom"/>
-      <geom pos="0.0713138 -0.0234828 0.108127" quat="0 -0.707107 0 -0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_3" contype="0" conaffinity="0" group="1" density="0"/>
-      <geom pos="0.100707 -0.0398328 0.0898537" quat="-0.707107 -0.707107 0 0" type="mesh" rgba="0.498039 0.498039 0.498039 1" mesh="LX-16A" class="visualgeom"/>
-      <geom pos="0.100707 -0.0398328 0.0898537" quat="-0.707107 -0.707107 0 0" type="mesh" rgba="0.498039 0.498039 0.498039 1" mesh="LX-16A" contype="0" conaffinity="0" group="1" density="0"/>
-
-      <geom pos="0.100707 -0.0398328 0.0196537" quat="-4.80472e-09 -4.80472e-09 -0.707107 0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_7" class="visualgeom"/>
-      <geom pos="0.100707 -0.0398328 0.0196537" quat="-4.80472e-09 -4.80472e-09 -0.707107 0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_7" contype="0" conaffinity="0" group="1" density="0"/>
-      <geom pos="-0.0713138 -0.0234828 0.108127" quat="0 -0.707107 0 0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_4" class="visualgeom"/>
-      <geom pos="-0.0713138 -0.0234828 0.108127" quat="0 -0.707107 0 0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_4" contype="0" conaffinity="0" group="1" density="0"/>
-      <geom pos="-0.100707 -0.0384328 0.0898537" quat="-0.707107 -0.707107 0 0" type="mesh" rgba="0.498039 0.498039 0.498039 1" mesh="LX-16A_2" class="visualgeom"/>
-      <geom pos="-0.100707 -0.0384328 0.0898537" quat="-0.707107 -0.707107 0 0" type="mesh" rgba="0.498039 0.498039 0.498039 1" mesh="LX-16A_2" contype="0" conaffinity="0" group="1" density="0"/>
-      <geom pos="-0.100707 -0.0384328 0.0196537" quat="-1.64085e-08 -1.64085e-08 0.707107 -0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_8"/>
-      <geom pos="-0.100707 -0.0384328 0.0196537" quat="-1.64085e-08 -1.64085e-08 0.707107 -0.707107" type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_8" contype="0" conaffinity="0" group="1" density="0"/>
-      <site name="imu" size="0.01" pos="0 0 0"/>
-      <body name="DRIVING_ROTOR_PLATE" pos="0.0413138 -0.0234828 -0.00437283" quat="1.64085e-08 -0.707107 -1.64085e-08 -0.707107">
-        <inertial pos="0.00739899 0.000707591 0.0199583" quat="0.575948 0.426026 0.459929 0.524643" mass="0.0679127" diaginertia="2.06887e-05 1.87407e-05 1.38506e-05"/>
-        <joint name="left_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-        <geom type="mesh" mesh="DRIVING_ROTOR_PLATE" class="visualgeom"/>
-        <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE" contype="0" conaffinity="0" group="1" density="0"/>
-        <body name="DRIVING_ROTOR_PLATE_5" pos="-0.0074 0.01695 0.02285" quat="0.5 -0.5 0.5 0.5">
-          <inertial pos="-1.51786e-06 0.0320531 -0.0325003" quat="0.706299 0.707914 -9.78407e-05 0.000106782" mass="0.19606" diaginertia="8.94807e-05 6.54785e-05 5.49377e-05"/>
-          <joint name="left_hip_yaw" pos="0 0 0" axis="0 0 -1" range="-1.5708 0.0872665" actuatorfrcrange="-1 1"/>
-          <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_5" class="visualgeom"/>
-          <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_5" contype="0" conaffinity="0" group="1" density="0"/>
-          <body name="DRIVING_ROTOR_PLATE_9" pos="-2.44814e-09 0.05275 -0.007" quat="1.64085e-08 1.64085e-08 -0.707107 -0.707107">
-            <inertial pos="-0.000259815 8.18298e-06 0.0247536" quat="0.707123 0.000568123 7.48082e-05 0.707091" mass="0.0598571" diaginertia="2.09913e-05 1.82322e-05 8.4096e-06"/>
-            <joint name="left_hip_roll" pos="0 0 0" axis="0 0 -1" range="-0.523599 0.523599" actuatorfrcrange="-1 1"/>
-            <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_9"  class="visualgeom"/>
-            <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_9" contype="0" conaffinity="0" group="1" density="0"/>
-            <body name="DRIVING_ROTOR_PLATE_11" pos="-0.0162309 1.02299e-09 0.0417" quat="0.5 0.5 -0.5 -0.5">
-              <inertial pos="1.53986e-05 0.0349186 -0.0157451" quat="0.730936 0.682446 0.000438709 0.000357984" mass="0.0704091" diaginertia="3.03594e-05 2.47989e-05 1.35071e-05"/>
-              <joint name="left_knee_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-              <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_11" class="visualgeom"/>
-              <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_11" contype="0" conaffinity="0" group="1" density="0"/>
-              <body name="DRIVING_ROTOR_PLATE_13" pos="-1.20434e-09 0.0552 -0.000319124" quat="1 0 0 0">
-                <inertial pos="-0.003644 0.0159002 -0.0152307" quat="0.500833 0.501995 0.497696 0.499466" mass="0.0450745" diaginertia="4.03647e-05 2.90427e-05 1.60029e-05"/>
-                <joint name="left_ankle_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-                <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_13" class="visualgeom"/>
-                <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_13"/>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-      <body name="DRIVING_ROTOR_PLATE_2" pos="-0.0413138 -0.0234828 -0.00437283" quat="2.58819e-08 -0.707107 2.58819e-08 0.707107">
-        <inertial pos="-0.00739212 0.000326566 0.0199574" quat="0.451732 0.537528 0.56406 0.434555" mass="0.0678925" diaginertia="2.06635e-05 1.87261e-05 1.38335e-05"/>
-        <joint name="right_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-        <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_2" class="visualgeom"/>
-        <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_2" contype="0" conaffinity="0" group="1" density="0"/>
-        <body name="DRIVING_ROTOR_PLATE_6" pos="0.0074 0.01645 0.02285" quat="0.5 -0.5 -0.5 -0.5">
-          <inertial pos="-1.51786e-06 0.0320531 -0.0325003" quat="0.706299 0.707914 -9.78407e-05 0.000106782" mass="0.19606" diaginertia="8.94807e-05 6.54785e-05 5.49377e-05"/>
-          <joint name="right_hip_yaw" pos="0 0 0" axis="0 0 -1" range="-0.0872665 1.5708" actuatorfrcrange="-1 1"/>
-          <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_6" class="visualgeom"/>
-          <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_6" contype="0" conaffinity="0" group="1" density="0"/>
-          <body name="DRIVING_ROTOR_PLATE_10" pos="-2.44814e-09 0.05275 -0.007" quat="1.64085e-08 1.64085e-08 -0.707107 -0.707107">
-            <inertial pos="0.000259815 -8.18299e-06 0.0247536" quat="0.707123 -0.000568123 -7.48082e-05 0.707091" mass="0.0598571" diaginertia="2.09913e-05 1.82322e-05 8.4096e-06"/>
-            <joint name="right_hip_roll" pos="0 0 0" axis="0 0 -1" range="-0.523599 0.523599" actuatorfrcrange="-1 1"/>
-            <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_10" class="visualgeom"/>
-            <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_10" contype="0" conaffinity="0" group="1" density="0"/>
-            <body name="DRIVING_ROTOR_PLATE_12" pos="0.0162309 -1.77627e-09 0.0417" quat="0.5 0.5 0.5 0.5">
-              <inertial pos="1.53986e-05 0.0349186 -0.0157451" quat="0.730936 0.682446 0.000438709 0.000357984" mass="0.0704091" diaginertia="3.03594e-05 2.47989e-05 1.35071e-05"/>
-              <joint name="right_knee_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-              <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_12" class="visualgeom"/>
-              <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_12" contype="0" conaffinity="0" group="1" density="0"/>
-              <body name="DRIVING_ROTOR_PLATE_14" pos="-1.20434e-09 0.0552 -0.000319124" quat="1 0 0 0">
-                <inertial pos="0.00362763 0.0158996 -0.0152306" quat="0.4977 0.499471 0.500827 0.501992" mass="0.0450552" diaginertia="4.03232e-05 2.90112e-05 1.59929e-05"/>
-                <joint name="right_ankle_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-1 1"/>
-                <geom type="mesh" mesh="DRIVING_ROTOR_PLATE_14" class="visualgeom"/>
-                <geom type="mesh" rgba="0.647059 0.647059 0.647059 1" mesh="DRIVING_ROTOR_PLATE_14"/>
-              </body>
-            </body>
-          </body>
-        </body>
-      </body>
-    </body>
-  </worldbody>
-  <sensor>
-    <actuatorpos name="left_hip_pitch_p" actuator="left_hip_pitch_motor" user="13"/>
-    <actuatorpos name="right_hip_pitch_p" actuator="right_hip_pitch_motor" user="13"/>
-    <actuatorpos name="left_hip_yaw_p" actuator="left_hip_yaw_motor" user="13"/>
-    <actuatorpos name="right_hip_yaw_p" actuator="right_hip_yaw_motor" user="13"/>
-    <actuatorpos name="left_hip_roll_p" actuator="left_hip_roll_motor" user="13"/>
-    <actuatorpos name="right_hip_roll_p" actuator="right_hip_roll_motor" user="13"/>
-    <actuatorpos name="left_knee_pitch_p" actuator="left_knee_pitch_motor" user="13"/>
-    <actuatorpos name="right_knee_pitch_p" actuator="right_knee_pitch_motor" user="13"/>
-    <actuatorpos name="left_ankle_pitch_p" actuator="left_ankle_pitch_motor" user="13"/>
-    <actuatorpos name="right_ankle_pitch_p" actuator="right_ankle_pitch_motor" user="13"/>
-
-    <!-- Actuator Velocity Sensors -->
-    <actuatorvel name="left_hip_pitch_v" actuator="left_hip_pitch_motor" user="13"/>
-    <actuatorvel name="right_hip_pitch_v" actuator="right_hip_pitch_motor" user="13"/>
-    <actuatorvel name="left_hip_yaw_v" actuator="left_hip_yaw_motor" user="13"/>
-    <actuatorvel name="right_hip_yaw_v" actuator="right_hip_yaw_motor" user="13"/>
-    <actuatorvel name="left_hip_roll_v" actuator="left_hip_roll_motor" user="13"/>
-    <actuatorvel name="right_hip_roll_v" actuator="right_hip_roll_motor" user="13"/>
-    <actuatorvel name="left_knee_pitch_v" actuator="left_knee_pitch_motor" user="13"/>
-    <actuatorvel name="right_knee_pitch_v" actuator="right_knee_pitch_motor" user="13"/>
-    <actuatorvel name="left_ankle_pitch_v" actuator="left_ankle_pitch_motor" user="13"/>
-    <actuatorvel name="right_ankle_pitch_v" actuator="right_ankle_pitch_motor" user="13"/>
-
-    <!-- Actuator Force Sensors -->
-    <actuatorfrc name="left_hip_pitch_f" actuator="left_hip_pitch_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="right_hip_pitch_f" actuator="right_hip_pitch_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="left_hip_yaw_f" actuator="left_hip_yaw_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="right_hip_yaw_f" actuator="right_hip_yaw_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="left_hip_roll_f" actuator="left_hip_roll_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="right_hip_roll_f" actuator="right_hip_roll_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="left_knee_pitch_f" actuator="left_knee_pitch_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="right_knee_pitch_f" actuator="right_knee_pitch_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="left_ankle_pitch_f" actuator="left_ankle_pitch_motor" user="13" noise="0.001"/>
-    <actuatorfrc name="right_ankle_pitch_f" actuator="right_ankle_pitch_motor" user="13" noise="0.001"/>
-
-    <!-- Attach sensors to the torso body -->
-    <framequat name="orientation" objtype="site" noise="0.001" objname="imu"/>
-    <gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9"/>
-  </sensor>
-
-  <actuator>
-    <motor name="left_hip_pitch_motor" joint="left_hip_pitch" ctrlrange="-100 100"/>
-    <motor name="right_hip_pitch_motor" joint="right_hip_pitch" ctrlrange="-100 100"/>
-    <motor name="left_hip_yaw_motor" joint="left_hip_yaw" ctrlrange="-100 100"/>
-    <motor name="right_hip_yaw_motor" joint="right_hip_yaw" ctrlrange="-100 100"/>
-    <motor name="left_hip_roll_motor" joint="left_hip_roll" ctrlrange="-100 100"/>
-    <motor name="right_hip_roll_motor" joint="right_hip_roll" ctrlrange="-100 100"/>
-    <motor name="left_knee_pitch_motor" joint="left_knee_pitch" ctrlrange="-100 100"/>
-    <motor name="right_knee_pitch_motor" joint="right_knee_pitch" ctrlrange="-100 100"/>
-    <motor name="left_ankle_pitch_motor" joint="left_ankle_pitch" ctrlrange="-100 100"/>
-    <motor name="right_ankle_pitch_motor" joint="right_ankle_pitch" ctrlrange="-100 100"/>
-  </actuator>
-
-  <keyframe>
-    <key name="default" qpos="0 0 0 0 0 0 0 0 0 0"/>
-</keyframe>
-
-</mujoco>
diff --git a/sim/resources/stompymicro/robot_fixed.xml b/sim/resources/stompymicro/robot_fixed.xml
index f078e8fd..a025b5aa 100644
--- a/sim/resources/stompymicro/robot_fixed.xml
+++ b/sim/resources/stompymicro/robot_fixed.xml
@@ -1,119 +1,139 @@
 <mujoco model="opus">
-  <option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81" />
-
-  <compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true" />
-
+  <compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true"/>
   <default>
-    <joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01" />
-    <geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2" />
-    <motor ctrllimited="true" />
-    <equality solref="0.001 2" />
+    <joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01"/>
+    <geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2"/>
+    <motor ctrllimited="true"/>
+    <equality solref="0.001 2"/>
     <default class="visualgeom">
-      <geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
+      <geom material="visualgeom" condim="1" contype="0" conaffinity="0"/>
     </default>
   </default>
-
+  <option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81"/>
   <asset>
-    <mesh name="Torso" file="Torso.stl" />
-    <mesh name="shoulder_yaw_right" file="shoulder_yaw_right.stl" />
-    <mesh name="shoulder_yaw_left" file="shoulder_yaw_left.stl" />
-    <mesh name="right_shoulder_yaw_motor" file="right_shoulder_yaw_motor.stl" />
-    <mesh name="hip_yaw_right" file="hip_yaw_right.stl" />
-    <mesh name="hip_yaw_left" file="hip_yaw_left.stl" />
-    <mesh name="left_shoulder_yaw_motor" file="left_shoulder_yaw_motor.stl" />
-    <mesh name="hip_roll_right" file="hip_roll_right.stl" />
-    <mesh name="hip_roll_left" file="hip_roll_left.stl" />
-    <mesh name="Left_Hand" file="Left_Hand.stl" />
-    <mesh name="hand_right" file="hand_right.stl" />
-    <mesh name="knee_pitch_right" file="knee_pitch_right.stl" />
-    <mesh name="knee_pitch_right_2" file="knee_pitch_right_2.stl" />
-    <mesh name="ankle_pitch_right" file="ankle_pitch_right.stl" />
-    <mesh name="ankle_pitch_left" file="ankle_pitch_left.stl" />
-    <mesh name="foot_right" file="foot_right.stl" />
-    <mesh name="foot_left" file="foot_left.stl" />
-    <texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="100" />
-    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true" />
-    <material name="visualgeom" rgba="0.5 0.9 0.2 1" />
+    <mesh name="Torso" file="Torso.stl"/>
+    <mesh name="right_shoulder_yaw" file="right_shoulder_yaw.stl"/>
+    <mesh name="left_shoulder_yaw" file="left_shoulder_yaw.stl"/>
+    <mesh name="right_shoulder_yaw_motor" file="right_shoulder_yaw_motor.stl"/>
+    <mesh name="hip_yaw_right" file="hip_yaw_right.stl"/>
+    <mesh name="hip_yaw_left" file="hip_yaw_left.stl"/>
+    <mesh name="left_shoulder_yaw_motor" file="left_shoulder_yaw_motor.stl"/>
+    <mesh name="hip_roll_right" file="hip_roll_right.stl"/>
+    <mesh name="hip_roll_left" file="hip_roll_left.stl"/>
+    <mesh name="right_hand" file="right_hand.stl"/>
+    <mesh name="left_hand" file="left_hand.stl"/>
+    <mesh name="knee_pitch_left" file="knee_pitch_left.stl"/>
+    <mesh name="knee_pitch_right" file="knee_pitch_right.stl"/>
+    <mesh name="right_knee_pitch_motor" file="right_knee_pitch_motor.stl"/>
+    <mesh name="left_knee_pitch_motor" file="left_knee_pitch_motor.stl"/>
+    <mesh name="foot_right" file="foot_right.stl"/>
+    <mesh name="foot_left" file="foot_left.stl"/>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="108"/>
+    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true"/>
+    <material name="visualgeom" rgba="0.5 0.9 0.2 1"/>
   </asset>
-
   <worldbody>
-    <light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false" />
-    <light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1" />
-    <geom name="ground" type="plane" pos="0 0 0" size="100 100 0.001" quat="1 0 0 0" material="matplane" condim="3" conaffinity="15" />
-    <camera name="fixed" pos="0 -3.0 0.5375878" xyaxes="1 0 0 0 0 1" />
-    <camera name="track" mode="trackcom" pos="0 -3.0 0.5375878" xyaxes="1 0 0 0 0 1" />
-    <body name="root" pos="0 0 0" quat="0.0 0.0 0.0 1.0">
-      <freejoint name="root" />
-      <site name="imu" size="0.01" pos="0 0 0" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Torso" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Torso" />
-      <geom pos="-0.0781365 -0.000335556 0.0430533" quat="0.5 0.5 0.5 0.5" type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_right" pos="-0.0781365 -0.000335556 0.0430533" quat="0.5 0.5 0.5 0.5" />
-      <geom pos="-0.108136 -0.0191606 0.0267541" quat="-0.707107 0.707107 9.47343e-09 9.47343e-09" type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_shoulder_yaw_motor" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_shoulder_yaw_motor" pos="-0.108136 -0.0191606 0.0267541" quat="-0.707107 0.707107 9.47343e-09 9.47343e-09" />
-      <geom pos="-0.13814 -0.0189106 -0.0375878" quat="-0.690346 0.690345 -0.153046 -0.153046" type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Left_Hand" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Left_Hand" pos="-0.13814 -0.0189106 -0.0375878" quat="-0.690346 0.690345 -0.153046 -0.153046" />
-      <geom pos="0.0781635 -0.000335556 0.0430533" quat="0.5 -0.5 -0.5 0.5" type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_left" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="shoulder_yaw_left" pos="0.0781635 -0.000335556 0.0430533" quat="0.5 -0.5 -0.5 0.5" />
-      <geom pos="0.108164 -0.0191606 0.0267541" quat="0.707107 0.707107 6.93506e-09 -6.93506e-09" type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_shoulder_yaw_motor" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_shoulder_yaw_motor" pos="0.108164 -0.0191606 0.0267541" quat="0.707107 0.707107 6.93506e-09 -6.93506e-09" />
-      <geom pos="0.138167 -0.0189106 -0.0375878" quat="0.690346 -0.690345 -0.153046 -0.153046" type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hand_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hand_right" pos="0.138167 -0.0189106 -0.0375878" quat="0.690346 -0.690345 -0.153046 -0.153046" />
-      <body name="hip_yaw_left" pos="0.0456635 -0.000335555 -0.0969467" quat="0.5 0.5 0.5 0.5">
-        <inertial pos="-0.00777616 0.0101887 0.0152718" quat="0.45592 0.889889 0.0131744 -0.00779523" mass="0.103202" diaginertia="4.31891e-05 4.07457e-05 3.38951e-05" />
-        <joint name="left_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" />
-        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_left" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_left" />
-        <body name="hip_roll_left" pos="-0.0255 -0.0025 0.01936" quat="1.64085e-08 -0.707107 -1.64085e-08 -0.707107">
-          <inertial pos="-5.22128e-05 0.0427184 0.0279626" quat="0.585474 0.810689 -0.00167161 -0.000214986" mass="0.111294" diaginertia="6.57191e-05 4.65391e-05 3.49746e-05" />
-          <joint name="left_hip_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 0.0872665" />
-          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_left" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_left" />
-          <body name="knee_pitch_right_2" pos="0 0.06776 0.015" quat="0.707107 0.707107 0 0">
-            <inertial pos="-0.000643821 -0.00237466 -0.0244388" quat="0.706931 0.0631453 -0.0485932 0.70278" mass="0.110612" diaginertia="7.52582e-05 6.81301e-05 2.60379e-05" />
-            <joint name="left_hip_roll" pos="0 0 0" axis="0 0 1" range="-0.785398 0.785398" />
-            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right_2" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right_2" />
-            <body name="ankle_pitch_left" pos="-0.018775 9.39161e-10 -0.04596" quat="0.5 -0.5 -0.5 0.5">
-              <inertial pos="2.15197e-06 0.0383739 -0.019579" quat="0.737365 0.675494 4.40773e-06 4.03788e-06" mass="0.109625" diaginertia="5.33242e-05 4.18066e-05 2.78814e-05" />
-              <joint name="left_knee_pitch" pos="0 0 0" axis="0 0 -1" range="0 1.0472" />
-              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_left" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_left" />
-              <body name="foot_left" pos="1.3459e-09 0.05647 -0.000450004" quat="2.32051e-08 -1 4.64102e-08 0">
-                <inertial pos="-0.00778227 -0.0161914 0.0178319" quat="0.488777 0.488777 0.510977 0.510977" mass="0.0406222" diaginertia="3.78658e-05 2.8894e-05 1.35969e-05" />
-                <joint name="left_ankle_pitch" pos="0 0 0" axis="0 0 1" range="-1.0472 1.0472" />
-                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_left" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_left" />
+    <geom name="ground" type="plane" pos="0.001 0 0" quat="1 0 0 0" material="matplane" condim="1" conaffinity="15" size="0 0 1"/>
+    <light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
+    <light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
+    <body name="root" pos="0 0 0" quat="0 0 0 1">
+      <freejoint name="root"/>
+      <camera name="front" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
+      <camera name="side" pos="-2.893 -1.330 0.757" xyaxes="0.405 -0.914 0.000 0.419 0.186 0.889" mode="trackcom"/>
+      <site name="imu" size="0.01" pos="0 0 0"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Torso" class="visualgeom"/>
+      <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="Torso"/>
+      <body name="right_shoulder_yaw_2" pos="-0.0782502 -0.000326898 0.0461512" quat="0.5 0.5 0.5 0.5">
+        <inertial pos="-0.000140403 -0.00296788 -0.0169439" quat="0.642185 0.295969 0.642185 0.295969" mass="0.0332955" diaginertia="1.98089e-05 1.70317e-05 8.43374e-06"/>
+        <joint name="right_shoulder_pitch" pos="0 0 0" axis="0 0 1" range="-1.74533 1.74533"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="right_shoulder_yaw" class="visualgeom"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="right_shoulder_yaw"/>
+        <body name="right_shoulder_yaw_motor" pos="-0.018825 -0.0162992 -0.03" quat="0.707107 0 -0.707107 0">
+          <inertial pos="-0.0150104 -0.0321876 -0.0177807" quat="0.689361 0.691294 -0.153343 -0.152904" mass="0.161288" diaginertia="0.000148011 0.000134022 3.73591e-05"/>
+          <joint name="right_shoulder_yaw" pos="0 0 0" axis="0 0 1" range="-1.13446 0.872665"/>
+          <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_shoulder_yaw_motor" class="visualgeom"/>
+          <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_shoulder_yaw_motor"/>
+          <body name="right_hand" pos="-0.0300031 -0.0643419 -0.000150004" quat="5.0225e-09 0.21644 0.976296 6.79651e-08">
+            <inertial pos="-3.1209e-05 -0.0458812 0.0183443" quat="0.725196 0.688542 0 0" mass="0.0576339" diaginertia="4.36057e-05 3.5573e-05 1.21088e-05"/>
+            <joint name="right_elbow_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708"/>
+            <geom type="mesh" rgba="0.980392 0.713726 0.00392157 1" mesh="right_hand" class="visualgeom"/>
+            <geom type="mesh" rgba="0.980392 0.713726 0.00392157 1" mesh="right_hand"/>
+          </body>
+        </body>
+      </body>
+      <body name="left_shoulder_yaw_2" pos="0.0780498 -0.000326898 0.0461512" quat="0.5 -0.5 -0.5 0.5">
+        <inertial pos="-0.00014038 0.00296788 -0.0169439" quat="0.295969 0.642186 0.295969 0.642186" mass="0.0332954" diaginertia="1.98089e-05 1.70317e-05 8.43373e-06"/>
+        <joint name="left_shoulder_pitch" pos="0 0 0" axis="0 0 1" range="-1.74533 1.74533"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="left_shoulder_yaw" class="visualgeom"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="left_shoulder_yaw"/>
+        <body name="left_shoulder_yaw_motor" pos="-0.018825 0.0162992 -0.03" quat="-1.64085e-08 0.707107 -1.64085e-08 -0.707107">
+          <inertial pos="0.0150104 -0.0321876 -0.0177807" quat="0.689377 0.691311 0.153267 0.152831" mass="0.161288" diaginertia="0.000147993 0.000134022 3.73767e-05"/>
+          <joint name="left_shoulder_yaw" pos="0 0 0" axis="0 0 1" range="-0.436332 1.5708"/>
+          <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_shoulder_yaw_motor" class="visualgeom"/>
+          <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_shoulder_yaw_motor"/>
+          <body name="left_hand" pos="0.0300031 -0.0643419 -0.000150001" quat="5.0225e-09 0.21644 -0.976296 -2.2655e-08">
+            <inertial pos="3.12423e-05 -0.045881 0.0183441" quat="0.725199 0.688539 0 0" mass="0.0576346" diaginertia="4.36062e-05 3.55733e-05 1.2109e-05"/>
+            <joint name="left_elbow_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708"/>
+            <geom type="mesh" rgba="0.980392 0.713726 0.00392157 1" mesh="left_hand" class="visualgeom"/>
+            <geom type="mesh" rgba="0.980392 0.713726 0.00392157 1" mesh="left_hand"/>
+          </body>
+        </body>
+      </body>
+      <body name="hip_yaw_right" pos="-0.0457502 -0.000326896 -0.103849" quat="0.5 -0.5 -0.5 0.5">
+        <inertial pos="-0.00782957 -0.000315357 0.0163297" quat="0.73746 0.674698 0.0153223 -0.0264575" mass="0.0932053" diaginertia="3.59902e-05 3.36518e-05 3.07706e-05"/>
+        <joint name="right_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_right" class="visualgeom"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_right"/>
+        <body name="hip_roll_right" pos="-0.0255 0.0125 0.01936" quat="0.707107 3.2817e-08 0.707107 -3.2817e-08">
+          <inertial pos="3.95624e-05 0.0445473 0.0271953" quat="0.557457 0.830204 0.00151221 0.000249092" mass="0.0992888" diaginertia="5.45822e-05 3.86078e-05 3.02163e-05"/>
+          <joint name="right_hip_yaw" pos="0 0 0" axis="0 0 1" range="-0.0872665 1.5708"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_right" class="visualgeom"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_right"/>
+          <body name="knee_pitch_left" pos="0 0.06776 0.015" quat="0.707107 -0.707107 0 0">
+            <inertial pos="0.000832072 0.00663444 -0.00535636" quat="0.702263 0.0826207 -0.0826207 0.702263" mass="0.0174156" diaginertia="1.12064e-05 8.67796e-06 4.51663e-06"/>
+            <joint name="right_hip_roll" pos="0 0 0" axis="0 0 -1" range="-0.785398 0.785398"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_left" class="visualgeom"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_left"/>
+            <body name="right_knee_pitch_motor" pos="0.018825 0 0.02221" quat="0.5 0.5 -0.5 -0.5">
+              <inertial pos="5.2853e-11 0.03886 0.017678" quat="0.707107 0.707107 2.09986e-05 2.09986e-05" mass="0.164074" diaginertia="0.000175989 0.000161732 3.83967e-05"/>
+              <joint name="right_knee_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708"/>
+              <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_knee_pitch_motor" class="visualgeom"/>
+              <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="right_knee_pitch_motor"/>
+              <body name="foot_right" pos="0 0.07772 2.08707e-09" quat="1 4.64102e-08 0 0">
+                <inertial pos="-0.00909654 0.0229469 0.0186337" quat="0.505668 0.505668 0.494267 0.494267" mass="0.0743148" diaginertia="7.16309e-05 6.04794e-05 1.96043e-05"/>
+                <joint name="right_ankle_pitch" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708"/>
+                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_right" class="visualgeom"/>
+                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_right"/>
               </body>
             </body>
           </body>
         </body>
       </body>
-        <body name="hip_yaw_right" pos="-0.0456365 -0.000335555 -0.0969467" quat="0.5 -0.5 -0.5 0.5">
-        <inertial pos="-0.00777521 -0.0101889 0.0152725" quat="0.889874 0.455956 0.00767715 -0.0129746" mass="0.103196" diaginertia="4.3188e-05 4.07436e-05 3.38931e-05" />
-        <joint name="right_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708" />
-        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_right" />
-        <body name="hip_roll_right" pos="-0.0255 0.0025 0.01936" quat="0.707107 3.2817e-08 0.707107 -3.2817e-08">
-          <inertial pos="5.22112e-05 0.0427183 0.0279625" quat="0.585477 0.810687 0.0016716 0.000214999" mass="0.111293" diaginertia="6.5719e-05 4.6539e-05 3.49745e-05" />
-          <joint name="right_hip_yaw" pos="0 0 0" axis="0 0 1" range="-0.0872665 1.5708" />
-          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_right" />
-          <body name="knee_pitch_right" pos="0 0.06776 0.015" quat="0.707107 0.707107 0 0">
-            <inertial pos="0.000718952 -0.00237466 -0.0219131" quat="0.702278 0.0508037 -0.0647173 0.707132" mass="0.110612" diaginertia="6.96382e-05 6.25773e-05 2.61051e-05" />
-            <joint name="right_hip_roll" pos="0 0 0" axis="0 0 1" range="-0.785398 0.785398" />
-            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right" />
-            <body name="ankle_pitch_right" pos="0.018825 -9.39161e-10 -0.04221" quat="0.5 -0.5 -0.5 0.5">
-              <inertial pos="-5.29027e-07 0.0409224 0.0178642" quat="0.709959 0.704243 3.3304e-05 3.30359e-05" mass="0.109625" diaginertia="5.75137e-05 4.60143e-05 2.78632e-05" />
-              <joint name="right_knee_pitch" pos="0 0 0" axis="0 0 1" range="-1.0472 0" />
-              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-              <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="ankle_pitch_right" />
-              <body name="foot_right" pos="0 0.06022 -1.3e-08" quat="2.32051e-08 -1 0 0">
-                <inertial pos="-0.00778227 -0.0161914 -0.0178319" quat="0.488777 0.488777 0.510977 0.510977" mass="0.0406222" diaginertia="3.78658e-05 2.8894e-05 1.35969e-05" />
-                <joint name="right_ankle_pitch" pos="0 0 0" axis="0 0 -1" range="-1.0472 1.0472" />
-                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_right" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
-                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_right" />
+      <body name="hip_yaw_left" pos="0.0455498 -0.000326896 -0.103849" quat="0.5 0.5 0.5 0.5">
+        <inertial pos="-0.00782956 0.000315355 0.0163297" quat="0.674699 0.73746 0.0264579 -0.0153225" mass="0.0932053" diaginertia="3.59902e-05 3.36518e-05 3.07706e-05"/>
+        <joint name="left_hip_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_left" class="visualgeom"/>
+        <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_yaw_left"/>
+        <body name="hip_roll_left" pos="-0.0255 -0.0125 0.01936" quat="1.64085e-08 -0.707107 -1.64085e-08 -0.707107">
+          <inertial pos="-3.95635e-05 0.0445472 0.0271953" quat="0.557458 0.830204 -0.00151221 -0.000249094" mass="0.0992889" diaginertia="5.45823e-05 3.86079e-05 3.02163e-05"/>
+          <joint name="left_hip_yaw" pos="0 0 0" axis="0 0 1" range="-1.5708 0.0872665"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_left" class="visualgeom"/>
+          <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="hip_roll_left"/>
+          <body name="knee_pitch_right" pos="0 0.06776 0.015" quat="0.707107 -0.707107 0 0">
+            <inertial pos="-0.000831894 0.00663441 -0.00535646" quat="0.702263 0.0826213 -0.0826213 0.702263" mass="0.0174154" diaginertia="1.12063e-05 8.67794e-06 4.51657e-06"/>
+            <joint name="left_hip_roll" pos="0 0 0" axis="0 0 -1" range="-0.785398 0.785398"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right" class="visualgeom"/>
+            <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="knee_pitch_right"/>
+            <body name="left_knee_pitch_motor" pos="-0.018825 0 0.02221" quat="0.5 -0.5 0.5 -0.5">
+              <inertial pos="-1.24277e-10 -0.03886 0.0174509" quat="0.704124 0.710078 -2.0918e-05 -2.10949e-05" mass="0.164074" diaginertia="0.000175991 0.00016174 3.83899e-05"/>
+              <joint name="left_knee_pitch" pos="0 0 0" axis="0 0 -1" range="-1.5708 1.5708"/>
+              <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_knee_pitch_motor" class="visualgeom"/>
+              <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="left_knee_pitch_motor"/>
+              <body name="foot_left" pos="0 -0.07772 -0.000499991" quat="1 4.64102e-08 0 0">
+                <inertial pos="-0.00909637 -0.0229469 0.0186334" quat="0.494267 0.494267 0.505668 0.505668" mass="0.0743146" diaginertia="7.16302e-05 6.04787e-05 1.96042e-05"/>
+                <joint name="left_ankle_pitch" pos="0 0 0" axis="0 0 1" range="-1.5708 1.5708"/>
+                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_left" class="visualgeom"/>
+                <geom type="mesh" rgba="0.972549 0.529412 0.00392157 1" mesh="foot_left"/>
               </body>
             </body>
           </body>
@@ -121,57 +141,36 @@
       </body>
     </body>
   </worldbody>
-
   <actuator>
-    <motor name="left_hip_pitch" joint="left_hip_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="left_hip_yaw" joint="left_hip_yaw" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="left_hip_roll" joint="left_hip_roll" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="left_knee_pitch" joint="left_knee_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="left_ankle_pitch" joint="left_ankle_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-  
-    <motor name="right_hip_pitch" joint="right_hip_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="right_hip_yaw" joint="right_hip_yaw" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="right_hip_roll" joint="right_hip_roll" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="right_knee_pitch" joint="right_knee_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-    <motor name="right_ankle_pitch" joint="right_ankle_pitch" ctrllimited="true" ctrlrange="-20 20" gear="1" />
-  </actuator>
+   <motor name="right_shoulder_pitch" joint="right_shoulder_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_elbow_yaw" joint="right_elbow_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    
+    
+    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_elbow_yaw" joint="left_elbow_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
 
+
+    <motor name="right_hip_pitch" joint="right_hip_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_hip_yaw" joint="right_hip_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_hip_roll" joint="right_hip_roll" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_knee_pitch" joint="right_knee_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="right_ankle_pitch" joint="right_ankle_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+
+    <motor name="left_hip_pitch" joint="left_hip_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_hip_yaw" joint="left_hip_yaw" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_hip_roll" joint="left_hip_roll" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_knee_pitch" joint="left_knee_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+    <motor name="left_ankle_pitch" joint="left_ankle_pitch" ctrllimited="true" ctrlrange="-200 200" gear="1" />
+  </actuator>
   <sensor>
-    <actuatorpos name="right_hip_pitch_p" actuator="right_hip_pitch" />
-    <actuatorvel name="right_hip_pitch_v" actuator="right_hip_pitch" />
-    <actuatorfrc name="right_hip_pitch_f" actuator="right_hip_pitch" noise="0.001" />
-    <actuatorpos name="right_hip_yaw_p" actuator="right_hip_yaw" />
-    <actuatorvel name="right_hip_yaw_v" actuator="right_hip_yaw" />
-    <actuatorfrc name="right_hip_yaw_f" actuator="right_hip_yaw" noise="0.001" />
-    <actuatorpos name="right_hip_roll_p" actuator="right_hip_roll" />
-    <actuatorvel name="right_hip_roll_v" actuator="right_hip_roll" />
-    <actuatorfrc name="right_hip_roll_f" actuator="right_hip_roll" noise="0.001" />
-    <actuatorpos name="right_knee_pitch_p" actuator="right_knee_pitch" />
-    <actuatorvel name="right_knee_pitch_v" actuator="right_knee_pitch" />
-    <actuatorfrc name="right_knee_pitch_f" actuator="right_knee_pitch" noise="0.001" />
-    <actuatorpos name="right_ankle_pitch_p" actuator="right_ankle_pitch" />
-    <actuatorvel name="right_ankle_pitch_v" actuator="right_ankle_pitch" />
-    <actuatorfrc name="right_ankle_pitch_f" actuator="right_ankle_pitch" noise="0.001" />
-    <actuatorpos name="left_hip_pitch_p" actuator="left_hip_pitch" />
-    <actuatorvel name="left_hip_pitch_v" actuator="left_hip_pitch" />
-    <actuatorfrc name="left_hip_pitch_f" actuator="left_hip_pitch" noise="0.001" />
-    <actuatorpos name="left_hip_yaw_p" actuator="left_hip_yaw" />
-    <actuatorvel name="left_hip_yaw_v" actuator="left_hip_yaw" />
-    <actuatorfrc name="left_hip_yaw_f" actuator="left_hip_yaw" noise="0.001" />
-    <actuatorpos name="left_hip_roll_p" actuator="left_hip_roll" />
-    <actuatorvel name="left_hip_roll_v" actuator="left_hip_roll" />
-    <actuatorfrc name="left_hip_roll_f" actuator="left_hip_roll" noise="0.001" />
-    <actuatorpos name="left_knee_pitch_p" actuator="left_knee_pitch" />
-    <actuatorvel name="left_knee_pitch_v" actuator="left_knee_pitch" />
-    <actuatorfrc name="left_knee_pitch_f" actuator="left_knee_pitch" noise="0.001" />
-    <actuatorpos name="left_ankle_pitch_p" actuator="left_ankle_pitch" />
-    <actuatorvel name="left_ankle_pitch_v" actuator="left_ankle_pitch" />
-    <actuatorfrc name="left_ankle_pitch_f" actuator="left_ankle_pitch" noise="0.001" />
-    <framequat name="orientation" objtype="site" noise="0.001" objname="imu" />
-    <gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9" />
+    <framequat name="orientation" objtype="site" noise="0.001" objname="imu"/>
+    <gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9"/>
   </sensor>
 
-  <keyframe>
-    <key name="default" qpos="0.0 0.0 0.293 1.0 0.0 0.0 0.0 0.23 0.0 0.0 0.741 -0.5 -0.23 0.0 0.0 -0.741 0.5" />
+    <keyframe>
+  <key name="default" qpos="0.0 0.0 0.293  0.9848 0.1736  0 0    0 0 0 0 0   0 0 0 0 0   0 0 0 0 0 0 " />
+    
   </keyframe>
-</mujoco>
+</mujoco>
\ No newline at end of file
diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py
index 7002eb9b..3dde8870 100755
--- a/sim/scripts/create_fixed_torso.py
+++ b/sim/scripts/create_fixed_torso.py
@@ -6,12 +6,13 @@
 from pathlib import Path
 
 from sim.scripts.create_mjcf import load_embodiment
-
+from sim.scripts.create_mjcf import create_mjcf
 
 def update_urdf(model_path: str, embodiment: str) -> None:
     tree = ET.parse(Path(model_path) / "robot.urdf")
     root = tree.getroot()
     robot = load_embodiment(embodiment)
+
     print(robot.default_standing())
     revolute_joints = set(robot.default_standing().keys())
 
@@ -57,7 +58,7 @@ def main() -> None:
     args = parser.parse_args()
 
     update_urdf(args.model_path, args.embodiment)
-    # create_mjcf(Path(args.model_path) / "robot")
+    create_mjcf(Path(args.model_path) / "robot")
 
 
 if __name__ == "__main__":