diff --git a/README.md b/README.md index 4a4f649..536ff27 100644 --- a/README.md +++ b/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. diff --git a/aze b/aze new file mode 100644 index 0000000..e69de29 diff --git a/example_config.json b/example_config.json new file mode 100644 index 0000000..7c7e381 --- /dev/null +++ b/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 + } +} \ No newline at end of file diff --git a/mini_bdx_runtime/assets/beep1.wav b/mini_bdx_runtime/assets/beep1.wav new file mode 100644 index 0000000..f6b7450 Binary files /dev/null and b/mini_bdx_runtime/assets/beep1.wav differ diff --git a/mini_bdx_runtime/assets/beep2.wav b/mini_bdx_runtime/assets/beep2.wav new file mode 100644 index 0000000..aea7ea3 Binary files /dev/null and b/mini_bdx_runtime/assets/beep2.wav differ diff --git a/mini_bdx_runtime/assets/happy1.wav b/mini_bdx_runtime/assets/happy1.wav new file mode 100644 index 0000000..f7174db Binary files /dev/null and b/mini_bdx_runtime/assets/happy1.wav differ diff --git a/mini_bdx_runtime/assets/happy2.wav b/mini_bdx_runtime/assets/happy2.wav new file mode 100644 index 0000000..ff772ab Binary files /dev/null and b/mini_bdx_runtime/assets/happy2.wav differ diff --git a/mini_bdx_runtime/assets/happy3.wav b/mini_bdx_runtime/assets/happy3.wav new file mode 100644 index 0000000..7f3124f Binary files /dev/null and b/mini_bdx_runtime/assets/happy3.wav differ diff --git a/mini_bdx_runtime/assets/lamp.wav b/mini_bdx_runtime/assets/lamp.wav new file mode 100644 index 0000000..a211b92 Binary files /dev/null and b/mini_bdx_runtime/assets/lamp.wav differ diff --git a/mini_bdx_runtime/assets/lamp2.wav b/mini_bdx_runtime/assets/lamp2.wav new file mode 100644 index 0000000..70bac5e Binary files /dev/null and b/mini_bdx_runtime/assets/lamp2.wav differ diff --git a/mini_bdx_runtime/assets/lamp3.wav b/mini_bdx_runtime/assets/lamp3.wav new file mode 100644 index 0000000..946b446 Binary files /dev/null and b/mini_bdx_runtime/assets/lamp3.wav differ diff --git a/mini_bdx_runtime/assets/motor.wav b/mini_bdx_runtime/assets/motor.wav new file mode 100644 index 0000000..5337bd0 Binary files /dev/null and b/mini_bdx_runtime/assets/motor.wav differ diff --git a/mini_bdx_runtime/mini_bdx_runtime/duck_config.py b/mini_bdx_runtime/mini_bdx_runtime/duck_config.py new file mode 100644 index 0000000..c570c15 --- /dev/null +++ b/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, + }, + ) 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 df148e9..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, 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 = { - # "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.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) @@ -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)) 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 987964c..0c945b5 100644 --- a/scripts/find_soft_offsets.py +++ b/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 = { - "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(dummy_config) + 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: diff --git a/scripts/head_puppet.py b/scripts/head_puppet.py index 8bf99cd..5f64bee 100644 --- a/scripts/head_puppet.py +++ b/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/") -antennas = Antennas() +duck_config = DuckConfig() -eyes = Eyes() -projector = Projector() -hwi = HWI() +xbox_controller = XBoxController(50, only_head_control=True) +buttons = Buttons() + +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,87 @@ 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 +try: + while True: + ( + 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) + 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) - if _p1.get_button(0): # A button - A_pressed = True + 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_pos_rad = np.deg2rad(head_pitch_deg) - if _p1.get_button(3): # X button - X_pressed = True + # 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) - # print("l_x", l_x) - # print("l_y", l_y) - # print("r_x", r_x) - # print("r_y", r_y) - # print("===") + 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) + if duck_config.antennas: + antennas.set_position_left(right_trigger) + antennas.set_position_right(left_trigger) - 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) + if buttons.B.triggered: + if duck_config.speaker: + sounds.play_random_sound() - 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) + if buttons.X.triggered: + if duck_config.projector: + projector.switch() - - 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) - - 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) - - - antennas.set_position_left(right_trigger) - antennas.set_position_right(left_trigger) - - if X_pressed: - sounds.play_random_sound() - if A_pressed: - projector.switch() - - - pygame.event.pump() # process event queue - time.sleep(1/60) + # pygame.event.pump() # process event queue + time.sleep(1 / 60) +except KeyboardInterrupt: + if duck_config.antennas: + antennas.stop() diff --git a/scripts/turn_off.py b/scripts/turn_off.py index 3d9c539..20d215c 100644 --- a/scripts/turn_off.py +++ b/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) diff --git a/scripts/turn_on.py b/scripts/turn_on.py index fe9de05..33edad3 100644 --- a/scripts/turn_on.py +++ b/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) diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index d8ebc85..d9e5f46 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/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 = [ - 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 = False - - 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 + self.phase_frequency_factor_offset = ( + self.duck_config.phase_frequency_factor_offset + ) - def add_fake_head(self, pos): - # add just the antennas now - assert len(pos) == self.num_dofs - pos_with_head = np.insert(pos, 9, [0, 0]) - return np.array(pos_with_head) + # 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 get_obs(self): @@ -198,6 +177,7 @@ class RLWalk: kps = [self.pid[0]] * 14 kds = [self.pid[2]] * 14 + # lower head kps kps[5:9] = [8, 8, 8, 8] self.hwi.set_kps(kps) @@ -251,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 @@ -271,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) - self.antennas.set_position_right(left_trigger) + if self.duck_config.antennas: + self.antennas.set_position_left(right_trigger) + self.antennas.set_position_right(left_trigger) if self.buttons.A.triggered: self.paused = not self.paused @@ -294,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( [ @@ -327,13 +316,13 @@ class RLWalk: self.motor_targets = self.init_pos + action * self.action_scale - self.motor_targets = np.clip( - self.motor_targets, - self.prev_motor_targets - - self.max_motor_velocity * (1 / self.control_freq), # control dt - self.prev_motor_targets - + self.max_motor_velocity * (1 / self.control_freq), # control dt - ) + # self.motor_targets = np.clip( + # self.motor_targets, + # self.prev_motor_targets + # - self.max_motor_velocity * (1 / self.control_freq), # control dt + # self.prev_motor_targets + # + self.max_motor_velocity * (1 / self.control_freq), # control dt + # ) if self.action_filter is not None: self.action_filter.push(self.motor_targets) @@ -348,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) @@ -364,7 +355,8 @@ class RLWalk: time.sleep(max(0, 1 / self.control_freq - took)) except KeyboardInterrupt: - pass + if self.duck_config.antennas: + self.antennas.stop() if self.save_obs: pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb")) @@ -376,8 +368,14 @@ 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=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) @@ -402,7 +400,7 @@ if __name__ == "__main__": default=None, help="replay the observations from a previous run (can be from the robot or from mujoco)", ) - parser.add_argument("--cutoff_frequency", type=float, default=40) + parser.add_argument("--cutoff_frequency", type=float, default=None) args = parser.parse_args() pid = [args.p, args.i, args.d] @@ -410,6 +408,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,