Merge pull request #5 from Codezombie23/v2

Added Config File Support

README.md

@@ -127,9 +127,17 @@ You can also run `python3 scripts/imu_server.py` on the robot and `python3 scrip
 
 > To find the ip address of the robot, run `ifconfig` on the robot
 
+## Make your duck_config.json
+
+Copy `example_config.json` in the home directory of your duck and rename it `duck_config.json`.
+
+`cp example_config.json ~/duck_config.json`
+
+In this file, you can configure some stuff, like registering if you installed the expression features, installed the imu upside down or and other stuff. You also write the joints offsets of your duck here
+
 ## Find the joints offsets
 
-This script will guide you through finding the joints offsets of your robot, that you can then write in `rustypot_position_hwi.py` in `self.joints_offsets`
+This script will guide you through finding the joints offsets of your robot that you can then write in your `duck_config.json`
 
 > This procedure won't be necessary in the future as we will be flashing the offsets directly in each motor's eeprom.
 

example_config.json

@@ -0,0 +1,29 @@
+{
+    "start_paused": false,
+    "imu_upside_down": false,
+    "phase_frequency_factor_offset": 0.0,
+    "expression_features": {
+        "eyes": false,
+        "projector": false,
+        "antennas": false,
+        "speaker": false,
+        "microphone": false,
+        "camera": false
+    },
+    "joints_offsets": {
+        "left_hip_yaw": 0.0,
+        "left_hip_roll": 0.0,
+        "left_hip_pitch": 0.0,
+        "left_knee": 0.0,
+        "left_ankle": 0.0,
+        "neck_pitch": 0.0,
+        "head_pitch": 0.0,
+        "head_yaw": 0.0,
+        "head_roll": 0.00,
+        "right_hip_yaw": 0.0,
+        "right_hip_roll": 0.0,
+        "right_hip_pitch": 0.0,
+        "right_knee": 0.0,
+        "right_ankle": 0.0
+    }
+}

mini_bdx_runtime/mini_bdx_runtime/duck_config.py

@@ -0,0 +1,83 @@
+import json
+from typing import Optional
+import os
+
+HOME_DIR = os.path.expanduser("~")
+
+
+class DuckConfig:
+
+    def __init__(
+        self,
+        config_json_path: Optional[str] = f"{HOME_DIR}/duck_config.json",
+        ignore_default: bool = False,
+    ):
+        """
+        Looks for duck_config.json in the home directory by default.
+        If not found, uses default values.
+        """
+        self.default = False
+        try:
+            self.json_config = (
+                json.load(open(config_json_path, "r")) if config_json_path else {}
+            )
+        except FileNotFoundError:
+            print(
+                f"Warning : didn't find the config json file at {config_json_path}, using default values"
+            )
+            self.json_config = {}
+            self.default = True
+
+        if config_json_path is None:
+            print("Warning : didn't provide a config json path, using default values")
+            self.default = True
+
+        if self.default and not ignore_default:
+            print("")
+            print("")
+            print("")
+            print("")
+            print("======")
+            print(
+                "WARNING : Running with default values probably won't work well. Please make a duck_config.json file and set the parameters."
+            )
+            res = input("Do you still want to run ? (y/N)")
+            if res.lower() != "y":
+                print("Exiting...")
+                exit(1)
+
+        self.start_paused = self.json_config.get("start_paused", False)
+        self.imu_upside_down = self.json_config.get("imu_upside_down", False)
+        self.phase_frequency_factor_offset = self.json_config.get(
+            "phase_frequency_factor_offset", 0.0
+        )
+
+        expression_features = self.json_config.get("expression_features", {})
+
+        self.eyes = expression_features.get("eyes", False)
+        self.projector = expression_features.get("projector", False)
+        self.antennas = expression_features.get("antennas", False)
+        self.speaker = expression_features.get("speaker", False)
+        self.microphone = expression_features.get("microphone", False)
+        self.camera = expression_features.get("camera", False)
+
+        # default joints offsets are 0.0
+        self.joints_offset = self.json_config.get(
+            "joints_offsets",
+            {
+                "left_hip_yaw": 0.0,
+                "left_hip_roll": 0.0,
+                "left_hip_pitch": 0.0,
+                "left_knee": 0.0,
+                "left_ankle": 0.0,
+                "neck_pitch": 0.0,
+                "head_pitch": 0.0,
+                "head_yaw": 0.0,
+                "head_roll": 0.00,
+                "right_hip_yaw": 0.0,
+                "right_hip_roll": 0.0,
+                "right_hip_pitch": 0.0,
+                "right_knee": 0.0,
+                "right_ankle": 0.0,
+            },
+        )

mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py

@@ -1,12 +1,14 @@
 import time
-from typing import List
 
 import numpy as np
 import rustypot
+from mini_bdx_runtime.duck_config import DuckConfig
 
 
 class HWI:
-    def __init__(self, usb_port="/dev/ttyACM0"):
+    def __init__(self, duck_config: DuckConfig, usb_port: str = "/dev/ttyACM0"):
+
+        self.duck_config = duck_config
 
         # Order matters here
         self.joints = {
@@ -66,87 +68,27 @@ class HWI:
             "right_ankle": -0.796,
         }
 
-        # self.joints_offsets = {
+        self.joints_offsets = self.duck_config.joints_offset
-        #     "left_hip_yaw": 0.084,
-        #     "left_hip_roll": -0.112,
-        #     "left_hip_pitch": -0.024,
-        #     "left_knee": 0.005,
-        #     "left_ankle": -0.09,
-        #     "neck_pitch": 0.017,
-        #     "head_pitch": 0.114,
-        #     "head_yaw": -0.083,
-        #     "head_roll": 0.072,
-        #     # "left_antenna": 0,
-        #     # "right_antenna": 0,
-        #     "right_hip_yaw": -0.13499999999999998,
-        #     "right_hip_roll": 0.205,
-        #     "right_hip_pitch": 0.064,
-        #     "right_knee": -0.027999999999999997,
-        #     "right_ankle": -0.09799999999999999,
-        # }
-        # self.joints_offsets = {
-        #     "left_hip_yaw": 0.14,
-        #     "left_hip_roll": -0.05,
-        #     "left_hip_pitch": -0.011,
-        #     "left_knee": 0.0,
-        #     "left_ankle": -0.107,
-        #     "neck_pitch": 0.061,
-        #     "head_pitch": 0.108,
-        #     "head_yaw": -0.08399999999999999,
-        #     "head_roll": 0.063,
-        #     "right_hip_yaw": -0.131,
-        #     "right_hip_roll": 0.089,
-        #     "right_hip_pitch": 0.06799999999999999,
-        #     "right_knee": 0.002,
-        #     "right_ankle": -0.093,
-        # }
-        self.joints_offsets = {
-            "left_hip_yaw" : 0.082,
-            "left_hip_roll" : -0.089,
-            "left_hip_pitch" : -0.004,
-            "left_knee" : 0.077,
-            "left_ankle" : -0.1,
-            "neck_pitch" : 0.057,
-            "head_pitch" : 0.069,
-            "head_yaw" : -0.101,
-            "head_roll" : 0.08,
-            "right_hip_yaw" : -0.166,
-            "right_hip_roll" : 0.115,
-            "right_hip_pitch" : 0.089,
-            "right_knee" : -0.098,
-            "right_ankle" : -0.125
-        }
-
-        init_pos_with_offsets = {
-            joint: np.rad2deg(pos + self.joints_offsets[joint])
-            for joint, pos in self.init_pos.items()
-        }
 
         self.kps = np.ones(len(self.joints)) * 32  # default kp
         self.kds = np.ones(len(self.joints)) * 0  # default kd
         self.low_torque_kps = np.ones(len(self.joints)) * 2
 
-        # self.control = rustypot.FeetechController(
-        #     usb_port, 1000000, 100, list(self.joints.values()), list(self.kps), list(init_pos_with_offsets.values())
-        # )
         self.io = rustypot.feetech(usb_port, 1000000)
 
     def set_kps(self, kps):
         self.kps = kps
         self.io.set_kps(list(self.joints.values()), self.kps)
-        # self.control.set_new_kps(self.kps)
 
     def set_kds(self, kds):
         self.kds = kds
         self.io.set_kds(list(self.joints.values()), self.kds)
 
     def set_kp(self, id, kp):
-        # self.kps[id] = kp
         self.io.set_kps([id], [kp])
 
     def turn_on(self):
         self.io.set_kps(list(self.joints.values()), self.low_torque_kps)
-        # self.control.set_new_kps(self.low_torque_kps)
         print("turn on : low KPS set")
         time.sleep(1)
 
@@ -160,10 +102,6 @@ class HWI:
 
     def turn_off(self):
         self.io.disable_torque(list(self.joints.values()))
-        # self.control.disable_torque()
-
-    # def freeze(self):
-    #     self.control.freeze()
 
     def set_position(self, joint_name, pos):
         """
@@ -172,7 +110,6 @@ class HWI:
         id = self.joints[joint_name]
         pos = pos + self.joints_offsets[joint_name]
         self.io.write_goal_position([id], [pos])
-        # self.control.set_new_target([pos])
 
     def set_position_all(self, joints_positions):
         """
@@ -187,23 +124,20 @@ class HWI:
         self.io.write_goal_position(
             list(self.joints.values()), list(ids_positions.values())
         )
-        # self.control.set_new_target(list(ids_positions.values()))
-        # self.control.goal_positions = list(ids_positions.values())
 
     def get_present_positions(self, ignore=[]):
         """
         Returns the present positions in radians
         """
 
-        # present_positions = np.deg2rad(
-        #     self.control.io.get_present_position(self.joints.values())
-        # )
         try:
-            present_positions = self.io.read_present_position(list(self.joints.values()))
+            present_positions = self.io.read_present_position(
+                list(self.joints.values())
+            )
         except Exception as e:
             print(e)
             return None
-        # present_positions = np.deg2rad(self.control.get_present_position())
+
         present_positions = [
             pos - self.joints_offsets[joint]
             for joint, pos in zip(self.joints.keys(), present_positions)
@@ -216,34 +150,17 @@ class HWI:
         Returns the present velocities in rad/s (default) or rev/min
         """
         try:
-            present_velocities = self.io.read_present_velocity(list(self.joints.values()))
+            present_velocities = self.io.read_present_velocity(
+                list(self.joints.values())
+            )
         except Exception as e:
             print(e)
             return None
-        # present_velocities = np.array(self.control.get_current_speed())
+
         present_velocities = [
             vel
             for joint, vel in zip(self.joints.keys(), present_velocities)
             if joint not in ignore
         ]
-        # present_velocities = np.array(
+
-        #     self.control.io.get_present_speed(self.joints.values())
-        # )
-        # if rad_s:
-        #     present_velocities = np.deg2rad(present_velocities)  # rad/s
         return np.array(np.around(present_velocities, 3))
-
-    # def get_present_voltages(self):
-    #     return np.array(self.control.io.get_present_voltage(self.joints.values())) * 0.1
-
-    # def get_present_velocities(self, rad_s=True):
-    #     """
-    #     Returns the present velocities in rad/s (default) or rev/min
-    #     """
-    #     # rev/min
-    #     present_velocities = np.array(
-    #         self.control.io.get_present_speed(self.joints.values())
-    #     )
-    #     if rad_s:
-    #         present_velocities = (2 * np.pi * present_velocities) / 60  # rad/s
-    #     return np.array(np.around(present_velocities, 3))

mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py

@@ -18,10 +18,10 @@ HEAD_ROLL_RANGE = [-0.5, 0.5]
 
 
 class XBoxController:
-    def __init__(self, command_freq, standing=False):
+    def __init__(self, command_freq, only_head_control=False):
         self.command_freq = command_freq
-        self.standing = standing
+        self.head_control_mode = only_head_control
-        self.head_control_mode = self.standing
+        self.only_head_control = only_head_control
 
         self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         self.last_left_trigger = 0.0
@@ -129,7 +129,8 @@ class XBoxController:
 
                 if self.p1.get_button(4):  # Y button
                     self.Y_pressed = True
-                    self.head_control_mode = not self.head_control_mode
+                    if not self.only_head_control:
+                        self.head_control_mode = not self.head_control_mode
 
                 if self.p1.get_button(6):  # LB button
                     self.LB_pressed = True
@@ -162,7 +163,7 @@ class XBoxController:
             self.RB_pressed,
             left_trigger,
             right_trigger,
-            up_down
+            up_down,
         )
 
     def get_last_command(self):
@@ -184,7 +185,7 @@ class XBoxController:
                 RB_pressed,
                 self.last_left_trigger,
                 self.last_right_trigger,
-                up_down
+                up_down,
             ) = self.cmd_queue.get(
                 False
             )  # non blocking
@@ -201,7 +202,7 @@ class XBoxController:
             RB_pressed,
             self.last_left_trigger,
             self.last_right_trigger,
-            up_down
+            up_down,
         )
 
 

scripts/find_soft_offsets.py

@@ -2,30 +2,24 @@
 Find the offsets to set in self.joints_offsets in hwi_feetech_pwm_control.py
 """
 
-# from mini_bdx_runtime.hwi_feetech_pwm_control import HWI
 from mini_bdx_runtime.rustypot_position_hwi import HWI
+from mini_bdx_runtime.duck_config import DuckConfig
 import time
 
+dummy_config = DuckConfig(config_json_path=None, ignore_default=True)
+
+print("======")
+print(
+    "Warning : this script will move the robot to its zero position quiclky, make sure it is safe to do so"
+)
+print("======")
+print("")
 input(
     "Press any key to start. The robot will move to its zero position. Make sure it is safe to do so. At any time, press ctrl+c to stop, the motors will be turned off."
 )
-hwi = HWI()
+
-hwi.joints_offsets = {
+hwi = HWI(dummy_config)
-    "left_hip_yaw": 0,
+
-    "left_hip_roll": 0,
-    "left_hip_pitch": 0,
-    "left_knee": 0,
-    "left_ankle": 0,
-    "neck_pitch": 0,
-    "head_pitch": 0,
-    "head_yaw": 0,
-    "head_roll": 0,
-    "right_hip_yaw": 0,
-    "right_hip_roll": 0,
-    "right_hip_pitch": 0,
-    "right_knee": 0,
-    "right_ankle": 0,
-}
 
 hwi.init_pos = hwi.zero_pos
 hwi.set_kds([0] * len(hwi.joints))
@@ -83,7 +77,7 @@ try:
             print("===")
 
     print("Done ! ")
-    print("Now you can copy the offsets in the hwi_feetech_pwm_control.py file")
+    print("Now you can copy the offsets in your duck_config.json")
 
 
 except KeyboardInterrupt:

scripts/head_puppet.py

@@ -2,23 +2,34 @@
 Sets up the robot in init position, you control the head with the xbox controller
 """
 
-from mini_bdx_runtime.rustypot_position_hwi import HWI
-from mini_bdx_runtime.eyes import Eyes
 import time
-import pygame
 import numpy as np
+from mini_bdx_runtime.rustypot_position_hwi import HWI
+from mini_bdx_runtime.duck_config import DuckConfig
+from mini_bdx_runtime.xbox_controller import XBoxController
+from mini_bdx_runtime.buttons import Buttons
 
 
+from mini_bdx_runtime.eyes import Eyes
 from mini_bdx_runtime.sounds import Sounds
 from mini_bdx_runtime.antennas import Antennas
 from mini_bdx_runtime.projector import Projector
 
-sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
+duck_config = DuckConfig()
-antennas = Antennas()
 
-eyes = Eyes()
+xbox_controller = XBoxController(50, only_head_control=True)
-projector = Projector()
+buttons = Buttons()
-hwi = HWI()
+
+if duck_config.speaker:
+    sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
+if duck_config.antennas:
+    antennas = Antennas()
+if duck_config.eyes:
+    eyes = Eyes()
+if duck_config.projector:
+    projector = Projector()
+
+hwi = HWI(duck_config)
 
 kps = [8] * 14
 kds = [0] * 14
@@ -27,81 +38,83 @@ hwi.set_kps(kps)
 hwi.set_kds(kds)
 hwi.turn_on()
 
-
-pygame.init()
-_p1 = pygame.joystick.Joystick(0)
-_p1.init()
-print(f"Loaded joystick with {_p1.get_numaxes()} axes.")
-
-
 limits = {
-    "neck_pitch" : [-20, 60],
+    "neck_pitch": [-20, 60],
-    "head_pitch" : [-60, 45],
+    "head_pitch": [-60, 45],
-    "head_yaw" : [-60, 60],
+    "head_yaw": [-60, 60],
-    "head_roll" : [-20, 20],
+    "head_roll": [-20, 20],
 }
 
-l_x = 0
-l_y = 0
-r_x = 0
-r_y = 0
-left_trigger = 0
-right_trigger = 0
 while True:
-    X_pressed = False
+    (
-    A_pressed = False
+        last_commands,
+        A_pressed,
+        B_pressed,
+        X_pressed,
+        Y_pressed,
+        LB_pressed,
+        RB_pressed,
+        left_trigger,
+        right_trigger,
+        up_down,
+    ) = xbox_controller.get_last_command()
 
-    for event in pygame.event.get():
+    buttons.update(
-        l_x = round(_p1.get_axis(0), 3)
+        A_pressed,
-        l_y = round(_p1.get_axis(1), 3)
+        B_pressed,
-        r_x = round(_p1.get_axis(2), 3)
+        X_pressed,
-        r_y = round(_p1.get_axis(3), 3)
+        Y_pressed,
+        LB_pressed,
+        RB_pressed,
+        up_down == 1,
+        up_down == -1,
+    )
 
+    l_x = last_commands[5]
+    l_y = last_commands[4]
+    r_x = last_commands[6]
+    # r_y = last_commands[3]
 
-        right_trigger = np.around((_p1.get_axis(4) + 1) / 2, 3)
+    head_yaw_deg = (
-        left_trigger = np.around((_p1.get_axis(5) + 1) / 2, 3)
+        l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2
-
+        + (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2
-        if _p1.get_button(0):  # A button
+    )
-            A_pressed = True
-
-
-        if _p1.get_button(3):  # X button
-            X_pressed = True
-
-    # print("l_x", l_x)
-    # print("l_y", l_y)
-    # print("r_x", r_x)
-    # print("r_y", r_y)
-    # print("===")
-
-
-    head_yaw_deg = l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2 + (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2
     head_yaw_pos_rad = np.deg2rad(head_yaw_deg)
 
-    head_roll_deg = r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2 + (limits["head_roll"][1] + limits["head_roll"][0]) / 2
+    head_roll_deg = (
+        r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2
+        + (limits["head_roll"][1] + limits["head_roll"][0]) / 2
+    )
     head_roll_pos_rad = np.deg2rad(head_roll_deg)
 
-
+    head_pitch_deg = (
-    head_pitch_deg = l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2 + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2
+        l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2
+        + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2
+    )
     head_pitch_pos_rad = np.deg2rad(head_pitch_deg)
 
-    neck_pitch_deg = -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2 + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2
+    # neck_pitch_deg = (
-    neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg)
+    #     -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2
+    #     + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2
+    # )
+    # neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg)
 
     hwi.set_position("head_yaw", head_yaw_pos_rad)
     hwi.set_position("head_roll", head_roll_pos_rad)
     hwi.set_position("head_pitch", head_pitch_pos_rad)
-    hwi.set_position("neck_pitch", neck_pitch_pos_rad)
+    # hwi.set_position("neck_pitch", neck_pitch_pos_rad)
 
+    if duck_config.antennas:
+        antennas.set_position_left(right_trigger)
+        antennas.set_position_right(left_trigger)
 
-    antennas.set_position_left(right_trigger)
+    if buttons.B.triggered:
-    antennas.set_position_right(left_trigger)
+        if duck_config.speaker:
+            sounds.play_random_sound()
 
-    if X_pressed:
+    if buttons.X.triggered:
-        sounds.play_random_sound()
+        if duck_config.projector:
-    if A_pressed:
+            projector.switch()
-        projector.switch()
 
-
+    # pygame.event.pump()  # process event queue
-    pygame.event.pump()  # process event queue
+    time.sleep(1 / 60)
-    time.sleep(1/60)

scripts/turn_off.py

@@ -1,4 +1,9 @@
 from mini_bdx_runtime.rustypot_position_hwi import HWI
+from mini_bdx_runtime.duck_config import DuckConfig
+import time
 
-hwi = HWI()
+duck_config = DuckConfig()
+
+hwi = HWI(duck_config)
 hwi.turn_off()
+time.sleep(1)

scripts/turn_on.py

@@ -1,6 +1,9 @@
 from mini_bdx_runtime.rustypot_position_hwi import HWI
+from mini_bdx_runtime.duck_config import DuckConfig
 import time
 
-hwi = HWI()
+duck_config = DuckConfig()
+
+hwi = HWI(duck_config)
 hwi.turn_on()
 time.sleep(1)

scripts/v2_rl_walk_mujoco.py

@@ -2,7 +2,6 @@ import time
 import pickle
 
 import numpy as np
-
 from mini_bdx_runtime.rustypot_position_hwi import HWI
 from mini_bdx_runtime.onnx_infer import OnnxInfer
 
@@ -16,15 +15,21 @@ from mini_bdx_runtime.antennas import Antennas
 from mini_bdx_runtime.projector import Projector
 from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter
 from mini_bdx_runtime.buttons import Buttons
+from mini_bdx_runtime.duck_config import DuckConfig
+
+import os
+
+HOME_DIR = os.path.expanduser("~")
 
 
 class RLWalk:
     def __init__(
         self,
         onnx_model_path: str,
+        duck_config_path: str = f"{HOME_DIR}/duck_config.json",
         serial_port: str = "/dev/ttyACM0",
         control_freq: float = 50,
-        pid=[32, 0, 0],
+        pid=[30, 0, 0],
         action_scale=0.25,
         commands=False,
         pitch_bias=0,
@@ -32,6 +37,9 @@ class RLWalk:
         replay_obs=None,
         cutoff_frequency=None,
     ):
+
+        self.duck_config = DuckConfig(config_json_path=duck_config_path)
+
         self.commands = commands
         self.pitch_bias = pitch_bias
 
@@ -45,25 +53,6 @@ class RLWalk:
         self.control_freq = control_freq
         self.pid = pid
 
-        self.joints_order = [
-            "left_hip_yaw",
-            "left_hip_roll",
-            "left_hip_pitch",
-            "left_knee",
-            "left_ankle",
-            "neck_pitch",
-            "head_pitch",
-            "head_yaw",
-            "head_roll",
-            # "left_antenna",
-            # "right_antenna",
-            "right_hip_yaw",
-            "right_hip_roll",
-            "right_hip_pitch",
-            "right_knee",
-            "right_ankle",
-        ]
-
         self.save_obs = save_obs
         if self.save_obs:
             self.saved_obs = []
@@ -78,18 +67,16 @@ class RLWalk:
                 self.control_freq, cutoff_frequency
             )
 
-        self.hwi = HWI(serial_port)
+        self.hwi = HWI(self.duck_config, serial_port)
+
         self.start()
 
         self.imu = Imu(
             sampling_freq=int(self.control_freq),
             user_pitch_bias=self.pitch_bias,
-            upside_down=False,
+            upside_down=self.duck_config.imu_upside_down,
         )
 
-        self.eyes = Eyes()
-        self.projector = Projector()
-
         self.feet_contacts = FeetContacts()
 
         # Scales
@@ -99,49 +86,41 @@ class RLWalk:
         self.last_last_action = np.zeros(self.num_dofs)
         self.last_last_last_action = np.zeros(self.num_dofs)
 
-        self.init_pos = [
+        self.init_pos = list(self.hwi.init_pos.values())
-            0.002,
-            0.053,
-            -0.63,
-            1.368,
-            -0.784,
-            0,
-            0,
-            0,
-            0,
-            -0.003,
-            -0.065,
-            0.635,
-            1.379,
-            -0.796,
-        ]
 
         self.motor_targets = np.array(self.init_pos.copy())
         self.prev_motor_targets = np.array(self.init_pos.copy())
 
         self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
-        self.paused = False
+        self.paused = self.duck_config.start_paused
-
-        self.sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/")
-        self.antennas = Antennas()
 
         self.command_freq = 20  # hz
         if self.commands:
             self.xbox_controller = XBoxController(self.command_freq)
             self.buttons = Buttons()
 
+        # Reference motion, but we only really need the length of one phase
+        # TODO
         self.PRM = PolyReferenceMotion("./polynomial_coefficients.pkl")
         self.imitation_i = 0
         self.imitation_phase = np.array([0, 0])
         self.phase_frequency_factor = 1.0
-        self.phase_frequency_factor_offset = 0.0
+        self.phase_frequency_factor_offset = (
+            self.duck_config.phase_frequency_factor_offset
+        )
 
-    def add_fake_head(self, pos):
+        # Optional expression features
-        # add just the antennas now
+        if self.duck_config.eyes:
-        assert len(pos) == self.num_dofs
+            self.eyes = Eyes()
-        pos_with_head = np.insert(pos, 9, [0, 0])
+        if self.duck_config.projector:
-        return np.array(pos_with_head)
+            self.projector = Projector()
+        if self.duck_config.speaker:
+            self.sounds = Sounds(
+                volume=1.0, sound_directory="../mini_bdx_runtime/assets/"
+            )
+        if self.duck_config.antennas:
+            self.antennas = Antennas()
 
     def get_obs(self):
 
@@ -252,16 +231,20 @@ class RLWalk:
                         LB_pressed,
                         RB_pressed,
                         up_down == 1,
-                        up_down == -1
+                        up_down == -1,
                     )
 
                     if self.buttons.dpad_up.triggered:
                         self.phase_frequency_factor_offset += 0.05
-                        print(f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}")
+                        print(
-                    
+                            f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}"
+                        )
+
                     if self.buttons.dpad_down.triggered:
                         self.phase_frequency_factor_offset -= 0.05
-                        print(f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}")
+                        print(
+                            f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}"
+                        )
 
                     if self.buttons.LB.is_pressed:
                         self.phase_frequency_factor = 1.3
@@ -272,13 +255,16 @@ class RLWalk:
                     # print(f"Phase frequency factor {self.phase_frequency_factor}")
 
                     if self.buttons.X.triggered:
-                        self.projector.switch()
+                        if self.duck_config.projector:
+                            self.projector.switch()
 
                     if self.buttons.B.triggered:
-                        self.sounds.play_random_sound()
+                        if self.duck_config.speaker:
+                            self.sounds.play_random_sound()
 
-                    self.antennas.set_position_left(right_trigger)
+                    if self.duck_config.antennas:
-                    self.antennas.set_position_right(left_trigger)
+                        self.antennas.set_position_left(right_trigger)
+                        self.antennas.set_position_right(left_trigger)
 
                     if self.buttons.A.triggered:
                         self.paused = not self.paused
@@ -295,7 +281,9 @@ class RLWalk:
                 if obs is None:
                     continue
 
-                self.imitation_i += 1 * (self.phase_frequency_factor + self.phase_frequency_factor_offset)
+                self.imitation_i += 1 * (
+                    self.phase_frequency_factor + self.phase_frequency_factor_offset
+                )
                 self.imitation_i = self.imitation_i % self.PRM.nb_steps_in_period
                 self.imitation_phase = np.array(
                     [
@@ -349,7 +337,9 @@ class RLWalk:
                 head_motor_targets = self.last_commands[3:] + self.motor_targets[5:9]
                 self.motor_targets[5:9] = head_motor_targets
 
-                action_dict = make_action_dict(self.motor_targets, self.joints_order)
+                action_dict = make_action_dict(
+                    self.motor_targets, list(self.hwi.joints.keys())
+                )
 
                 self.hwi.set_position_all(action_dict)
 
@@ -377,6 +367,12 @@ if __name__ == "__main__":
 
     parser = argparse.ArgumentParser()
     parser.add_argument("--onnx_model_path", type=str, required=True)
+    parser.add_argument(
+        "--duck_config_path",
+        type=str,
+        required=False,
+        default=f"{HOME_DIR}/duck_config.json",
+    )
     parser.add_argument("-a", "--action_scale", type=float, default=0.25)
     parser.add_argument("-p", type=int, default=30)
     parser.add_argument("-i", type=int, default=0)
@@ -411,6 +407,7 @@ if __name__ == "__main__":
     print("Done parsing args")
     rl_walk = RLWalk(
         args.onnx_model_path,
+        duck_config_path=args.duck_config_path,
         action_scale=args.action_scale,
         pid=pid,
         control_freq=args.control_freq,