diff --git a/example_config.json b/example_config.json index 7c7e381..368a772 100644 --- a/example_config.json +++ b/example_config.json @@ -1,4 +1,5 @@ { + "serial_port": "/dev/ttyACM0", "start_paused": false, "imu_upside_down": false, "phase_frequency_factor_offset": 0.0, diff --git a/mini_bdx_runtime/mini_bdx_runtime/duck_config.py b/mini_bdx_runtime/mini_bdx_runtime/duck_config.py index c570c15..f381eab 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/duck_config.py +++ b/mini_bdx_runtime/mini_bdx_runtime/duck_config.py @@ -46,6 +46,7 @@ class DuckConfig: print("Exiting...") exit(1) + self.serial_port = self.json_config.get("serial_port", "/dev/ttyACM0") 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( 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 c065c8f..58e9f29 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py +++ b/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py @@ -6,7 +6,7 @@ from mini_bdx_runtime.duck_config import DuckConfig class HWI: - def __init__(self, duck_config: DuckConfig, usb_port: str = "/dev/ttyACM0"): + def __init__(self, duck_config: DuckConfig): self.duck_config = duck_config @@ -74,7 +74,7 @@ class HWI: self.kds = np.ones(len(self.joints)) * 0 # default kd self.low_torque_kps = np.ones(len(self.joints)) * 2 - self.io = rustypot.feetech(usb_port, 1000000) + self.io = rustypot.feetech(self.duck_config.serial_port, 1000000) def set_kps(self, kps): self.kps = kps diff --git a/mini_bdx_runtime/mini_bdx_runtime/utils.py b/mini_bdx_runtime/mini_bdx_runtime/utils.py new file mode 100644 index 0000000..e958350 --- /dev/null +++ b/mini_bdx_runtime/mini_bdx_runtime/utils.py @@ -0,0 +1,51 @@ +import serial.tools.list_ports +from pypot.feetech import FeetechSTS3215IO + +DEVICE_IDS = [ + (0x1A86, 0x55D3), # Example device 1 + # Add more (vendor_id, product_id) pairs here as needed +] + +FALLBACK_PORT = "/dev/ttyACM0" + + +def get_port(): + port = None + auto_port = auto_detect_port() + if auto_port is None: + print( + f"Device not auto-detected. Attempting connection using fallback port {FALLBACK_PORT}." + ) + port = FALLBACK_PORT + else: + port = auto_port + + # Attempt to connect and catch any connection errors. + try: + FeetechSTS3215IO(port) + return port + except Exception as exc: + message = f"Error connecting to the motor using port {port}: {exc}. Please check your connection.\n" + # If the fallback port was used, advise the user with additional usage information. + if port == FALLBACK_PORT: + message += f" If your device is not connected via {FALLBACK_PORT}, please specify the correct port using the '--port' argument (e.g., --port /dev/ttyUSB0)." + else: + message += " If you believe your device is connected on a different port, try specifying it using the '--port ' argument." + print(message) + return None + + +def auto_detect_port(): + """ + Scans available serial ports and returns the port name for the first device + matching any of the given (vendor_id, product_id) pairs. + """ + ports = list(serial.tools.list_ports.comports()) + for port in ports: + for vendor_id, product_id in DEVICE_IDS: + if port.vid == vendor_id and port.pid == product_id: + print( + f"Found device on port: {port.device} (VID: {hex(vendor_id)}, PID: {hex(product_id)})" + ) + return port.device + return None diff --git a/scripts/configure_motor.py b/scripts/configure_motor.py index c95639f..d62a73c 100644 --- a/scripts/configure_motor.py +++ b/scripts/configure_motor.py @@ -1,27 +1,41 @@ from pypot.feetech import FeetechSTS3215IO import argparse import time +from mini_bdx_runtime.utils import get_port +import tqdm DEFAULT_ID = 1 # A brand new motor should have id 1 +# Parse arguments. This allows an override if you supply --port. parser = argparse.ArgumentParser() parser.add_argument( "--port", - help="The port the motor is connected to. Default is /dev/ttyACM0. Use `ls /dev/tty* | grep usb` to find the port.", - default="/dev/ttyACM0", + help=( + "The port the motor is connected to. If not specified, " + "the script will try to auto-detect the port using provided USB IDs or " + "fall back to /dev/ttyACM0." + ), + default=None, ) parser.add_argument("--id", help="The id to set to the motor.", type=str, required=True) args = parser.parse_args() -io = FeetechSTS3215IO(args.port) + +port = args.port +# If no port is provided, try to auto-detect using the USB id info. +if port is None: + port = get_port() +if port is None: + exit() + + +io = FeetechSTS3215IO(port) current_id = DEFAULT_ID def scan(): id = None - for i in range(255): - - print(f"scanning for id {i} ...") + for i in tqdm.tqdm(range(255), desc="Scanning ...", unit="id"): try: io.get_present_position([i]) id = i @@ -39,6 +53,7 @@ except Exception: f"Could not find motor with default id ({DEFAULT_ID}). Scanning for motor ..." ) res = scan() + print("ID :", res) if res is not None: current_id = res else: @@ -46,8 +61,6 @@ except Exception: exit() -# print("current id: ", current_id) - kp = io.get_P_coefficient([current_id]) ki = io.get_I_coefficient([current_id]) kd = io.get_D_coefficient([current_id]) @@ -55,11 +68,6 @@ max_acceleration = io.get_maximum_acceleration([current_id]) acceleration = io.get_acceleration([current_id]) mode = io.get_mode([current_id]) -# print(f"PID : {kp}, {ki}, {kd}") -# print(f"max_acceleration: {max_acceleration}") -# print(f"acceleration: {acceleration}") -# print(f"mode: {mode}") - io.set_lock({current_id: 0}) io.set_mode({current_id: 0}) io.set_maximum_acceleration({current_id: 0}) @@ -72,7 +80,13 @@ io.change_id({current_id: int(args.id)}) current_id = int(args.id) time.sleep(1) +print("==") +res = input("WARNING, the motor will move to its zero position. Continue ? ([Y]/n)") +if res.lower() != "y" and res.lower() != "": + print("Exiting ...") + exit() +print("==") io.set_goal_position({current_id: 0}) time.sleep(1) diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index 530ebb9..ad11c8b 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/scripts/v2_rl_walk_mujoco.py @@ -26,7 +26,6 @@ class RLWalk: 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=[30, 0, 0], action_scale=0.25, @@ -66,7 +65,7 @@ class RLWalk: self.control_freq, cutoff_frequency ) - self.hwi = HWI(self.duck_config, serial_port) + self.hwi = HWI(self.duck_config) self.start() diff --git a/setup.cfg b/setup.cfg index 0eb07fa..3f00a52 100644 --- a/setup.cfg +++ b/setup.cfg @@ -24,6 +24,7 @@ install_requires = pygame==2.6.0 pypot @ git+https://github.com/pollen-robotics/pypot@support-feetech-sts3215 openai==1.70.0 + tqdm # adafruit_extended_bus