From 08c7867b1e839e594812e5f81b01fea14599f913 Mon Sep 17 00:00:00 2001 From: apirrone Date: Sat, 19 Apr 2025 12:02:58 +0200 Subject: [PATCH] =?UTF-8?q?[WIP]=C2=A0propagating=20and=20cleaning=20stuff?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mini_bdx_runtime/duck_config.py | 18 +- .../mini_bdx_runtime/rustypot_position_hwi.py | 96 ++--------- .../mini_bdx_runtime/xbox_controller.py | 15 +- scripts/find_soft_offsets.py | 29 ++-- scripts/head_puppet.py | 157 +++++++++-------- scripts/turn_off.py | 22 +-- scripts/turn_on.py | 20 +-- scripts/v2_rl_walk_mujoco.py | 159 +++++------------- 8 files changed, 173 insertions(+), 343 deletions(-) diff --git a/mini_bdx_runtime/mini_bdx_runtime/duck_config.py b/mini_bdx_runtime/mini_bdx_runtime/duck_config.py index a613123..454a5bd 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/duck_config.py +++ b/mini_bdx_runtime/mini_bdx_runtime/duck_config.py @@ -3,10 +3,20 @@ from typing import Optional class DuckConfig: - def __init__(self, config_json_path: Optional[str] = None): - self.json_config = ( - json.load(open(config_json_path, "r")) if config_json_path else {} - ) + def __init__(self, config_json_path: Optional[str] = "~/duck_config.json"): + """ + Looks for duck_config.json in the home directory by default. + If not found, uses default values. + """ + 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 = {} if config_json_path is None: print("Warning : didn't provide a config json path, using default values") diff --git a/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py b/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py index 7f5d996..c065c8f 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py +++ b/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, offsets, 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,72 +68,27 @@ class HWI: "right_ankle": -0.796, } - # self.joints_offsets = { - # "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 = offsets - - init_pos_with_offsets = { - joint: np.rad2deg(pos + self.joints_offsets[joint]) - for joint, pos in self.init_pos.items() - } + self.joints_offsets = self.duck_config.joints_offset 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) @@ -145,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): """ @@ -157,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): """ @@ -172,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) @@ -201,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)) diff --git a/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py b/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py index 7a6bf6b..2a90abf 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py +++ b/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 = self.standing + self.head_control_mode = only_head_control + 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, ) diff --git a/scripts/find_soft_offsets.py b/scripts/find_soft_offsets.py index c855702..c43df0e 100644 --- a/scripts/find_soft_offsets.py +++ b/scripts/find_soft_offsets.py @@ -4,29 +4,22 @@ 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) + +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." ) -joints_offsets = { - "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 = HWI(joints_offsets) +hwi = HWI(dummy_config) hwi.init_pos = hwi.zero_pos @@ -85,7 +78,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: diff --git a/scripts/head_puppet.py b/scripts/head_puppet.py index 2ed65d7..5e44b16 100644 --- a/scripts/head_puppet.py +++ b/scripts/head_puppet.py @@ -2,41 +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/") -antennas = Antennas() +duck_config = DuckConfig() -eyes = Eyes() -projector = Projector() +xbox_controller = XBoxController(50, only_head_control=True) +buttons = Buttons() -joints_offsets = { - "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, -} +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(joints_offsets) +hwi = HWI(duck_config) kps = [8] * 14 kds = [0] * 14 @@ -45,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], - "head_pitch" : [-60, 45], - "head_yaw" : [-60, 60], - "head_roll" : [-20, 20], + "neck_pitch": [-20, 60], + "head_pitch": [-60, 45], + "head_yaw": [-60, 60], + "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(): - l_x = round(_p1.get_axis(0), 3) - l_y = round(_p1.get_axis(1), 3) - r_x = round(_p1.get_axis(2), 3) - r_y = round(_p1.get_axis(3), 3) + buttons.update( + A_pressed, + B_pressed, + X_pressed, + 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) - left_trigger = np.around((_p1.get_axis(5) + 1) / 2, 3) - - 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_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 = l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2 + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2 + head_pitch_deg = ( + 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_pos_rad = np.deg2rad(neck_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_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) - antennas.set_position_right(left_trigger) + if buttons.B.triggered: + if duck_config.speaker: + sounds.play_random_sound() - if X_pressed: - sounds.play_random_sound() - if A_pressed: - projector.switch() + if buttons.X.triggered: + if duck_config.projector: + projector.switch() - - pygame.event.pump() # process event queue - time.sleep(1/60) + # pygame.event.pump() # process event queue + time.sleep(1 / 60) diff --git a/scripts/turn_off.py b/scripts/turn_off.py index 88eaa71..20d215c 100644 --- a/scripts/turn_off.py +++ b/scripts/turn_off.py @@ -1,21 +1,9 @@ from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time -joints_offsets = { - "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, -} +duck_config = DuckConfig() -hwi = HWI(joints_offsets) +hwi = HWI(duck_config) hwi.turn_off() +time.sleep(1) diff --git a/scripts/turn_on.py b/scripts/turn_on.py index ba14ccd..33edad3 100644 --- a/scripts/turn_on.py +++ b/scripts/turn_on.py @@ -1,23 +1,9 @@ from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig import time -joints_offsets = { - "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, -} +duck_config = DuckConfig() -hwi = HWI(joints_offsets) +hwi = HWI(duck_config) hwi.turn_on() time.sleep(1) diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index ba571a2..91c01cc 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/scripts/v2_rl_walk_mujoco.py @@ -20,15 +20,17 @@ 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 class RLWalk: def __init__( self, onnx_model_path: str, + duck_config_path: str = "~/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, @@ -36,6 +38,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 @@ -49,25 +54,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 = [] @@ -82,84 +68,16 @@ class RLWalk: self.control_freq, cutoff_frequency ) - - - IMU_upside_down = False - self.phase_frequency_factor_offset = 0.0 - - # Read config file - print("Read Config") - try: - with open('config.json') as f: - buf = f.read() - config_data = json.loads(buf) - print("Ok") - f.close() - except: - # check file exists if it does'nt create an empty config. - if os.path.isfile("config.json") == False: - with open("config.json","w") as newFile: - newFile.write("{\n") - newFile.write(" ""start_paused"": false,\n") - newFile.write(" ""imu_upside_down"": false,\n") - newFile.write(" ""phase_frequency_factor_offset"": 0.0,\n") - newFile.write(" ""left_hip_yaw"": 0.0,\n") - newFile.write(" ""left_hip_roll"": 0.0,\n") - newFile.write(" ""left_hip_pitch"": 0.0,\n") - newFile.write(" ""left_knee"": 0.0,\n") - newFile.write(" ""left_ankle"": 0.0,\n") - newFile.write(" ""neck_pitch"": 0.0,\n") - newFile.write(" ""head_pitch"": 0.0,\n") - newFile.write(" ""head_yaw"": 0.0,\n") - newFile.write(" ""head_roll"": 0.0,\n") - newFile.write(" ""right_hip_yaw"": 0.0,\n") - newFile.write(" ""right_hip_roll"": 0.0,\n") - newFile.write(" ""right_hip_pitch"": 0.0,\n") - newFile.write(" ""right_knee"": 0.0,\n") - newFile.write(" ""right_ankle"": 0.0\n") - newFile.write("}") - newFile.close() - print("Config not found, new config.json file created, aborting execution") - print("Fill out config file with motor offsets using find_soft_offsets.py") - sys.exit() - - - joint_offsets = { - "left_hip_yaw": config_data.get("left_hip_yaw"), - "left_hip_roll": config_data.get("left_hip_roll"), - "left_hip_pitch": config_data.get("left_hip_pitch"), - "left_knee": config_data.get("left_knee"), - "left_ankle": config_data.get("left_ankle"), - "neck_pitch": config_data.get("neck_pitch"), - "head_pitch": config_data.get("head_pitch"), - "head_yaw": config_data.get("head_yaw"), - "head_roll": config_data.get("head_roll"), - "right_hip_yaw": config_data.get("right_hip_yaw"), - "right_hip_roll": config_data.get("right_hip_roll"), - "right_hip_pitch": config_data.get("right_hip_pitch"), - "right_knee": config_data.get("right_knee"), - "right_ankle": config_data.get("right_ankle") - } - - self.hwi = HWI(joint_offsets, serial_port) - - IMU_upside_down = config_data.get("imu_upside_down") - self.phase_frequency_factor_offset = config_data.get("phase_frequency_factor_offset") - + self.hwi = HWI(self.duck_config, serial_port) - - # end Config load section self.start() self.imu = Imu( sampling_freq=int(self.control_freq), user_pitch_bias=self.pitch_bias, - upside_down=IMU_upside_down, + upside_down=self.duck_config.imu_upside_down, ) - self.eyes = Eyes() - self.projector = Projector() - self.feet_contacts = FeetContacts() # Scales @@ -169,44 +87,39 @@ class RLWalk: self.last_last_action = np.zeros(self.num_dofs) self.last_last_last_action = np.zeros(self.num_dofs) - self.init_pos = [ - 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.init_pos = list(self.hwi.init_pos.values()) 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 = config_data.get("start_paused") - - self.sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/") - self.antennas = Antennas() + self.paused = self.duck_config.start_paused 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 + # Optional expression features + if self.duck_config.eyes: + self.eyes = Eyes() + if self.duck_config.projector: + 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 add_fake_head(self, pos): # add just the antennas now @@ -322,16 +235,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 @@ -365,7 +282,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( [ @@ -419,7 +338,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) @@ -447,8 +368,11 @@ 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="~/duck_config.json" + ) parser.add_argument("-a", "--action_scale", type=float, default=0.25) - parser.add_argument("-p", type=int, default=32) + parser.add_argument("-p", type=int, default=30) parser.add_argument("-i", type=int, default=0) parser.add_argument("-d", type=int, default=0) parser.add_argument("-c", "--control_freq", type=int, default=50) @@ -481,6 +405,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,