From 189124dc9207b61a6db1b05fc56cf05db76f0594 Mon Sep 17 00:00:00 2001 From: Jaime Machuca Date: Thu, 10 Apr 2025 18:24:09 -0600 Subject: [PATCH 1/5] Add automatic port detction for waveshare boards. --- scripts/configure_motor.py | 52 +++++++++++++++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/scripts/configure_motor.py b/scripts/configure_motor.py index c95639f..4f292c6 100644 --- a/scripts/configure_motor.py +++ b/scripts/configure_motor.py @@ -1,18 +1,64 @@ from pypot.feetech import FeetechSTS3215IO import argparse import time +import serial.tools.list_ports DEFAULT_ID = 1 # A brand new motor should have id 1 +# These are placeholders. Please replace these with your actual device's USB IDs. +TARGET_VENDOR_ID = 0x1A86 # e.g., your device's vendor id (in hex) +TARGET_PRODUCT_ID = 0x55D3 # e.g., your device's product id (in hex) + +def find_port(vendor_id, product_id): + """ + Scans available serial ports and returns the port name for the device + matching the given vendor_id and product_id. + """ + ports = list(serial.tools.list_ports.comports()) + for port in ports: + # Check if the port has the specified vendor_id and product_id + #print(f"Found port: {port.device}, VID: {hex(port.vid) if port.vid is not None else 'None'}, PID: {hex(port.pid) if port.pid is not None else 'None'}") + # port.vid and port.pid are provided by pyserial if available + if port.vid == vendor_id and port.pid == product_id: + print(f"Found device on port: {port.device}") + return port.device + return None + +# 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) + +# If no port is provided, try to auto-detect using the USB id info. +if args.port is None: + auto_port = find_port(TARGET_VENDOR_ID, TARGET_PRODUCT_ID) + if auto_port is None: + fallback_port = "/dev/ttyACM0" + print(f"Device not auto-detected. Attempting connection using fallback port {fallback_port}.") + args.port = fallback_port + else: + args.port = auto_port + +# Attempt to connect and catch any connection errors. +try: + io = FeetechSTS3215IO(args.port) +except Exception as exc: + message = f"Error connecting to the motor using port {args.port}: {exc}. Please check your connection.\n" + # If the fallback port was used, advise the user with additional usage information. + fallback_port = "/dev/ttyACM0" + if args.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) + exit(1) current_id = DEFAULT_ID From 40a14bccf6c074b792881e5632eb589bea59ab69 Mon Sep 17 00:00:00 2001 From: Jaime Machuca Date: Thu, 10 Apr 2025 18:28:40 -0600 Subject: [PATCH 2/5] Allow multiple boards vid/pid --- scripts/configure_motor.py | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/scripts/configure_motor.py b/scripts/configure_motor.py index 4f292c6..2110d78 100644 --- a/scripts/configure_motor.py +++ b/scripts/configure_motor.py @@ -9,21 +9,26 @@ DEFAULT_ID = 1 # A brand new motor should have id 1 TARGET_VENDOR_ID = 0x1A86 # e.g., your device's vendor id (in hex) TARGET_PRODUCT_ID = 0x55D3 # e.g., your device's product id (in hex) -def find_port(vendor_id, product_id): +# Update the find_port function to accept a list of (vendor_id, product_id) tuples. +def find_port(device_ids): """ - Scans available serial ports and returns the port name for the device - matching the given vendor_id and product_id. + 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: - # Check if the port has the specified vendor_id and product_id - #print(f"Found port: {port.device}, VID: {hex(port.vid) if port.vid is not None else 'None'}, PID: {hex(port.pid) if port.pid is not None else 'None'}") - # port.vid and port.pid are provided by pyserial if available - if port.vid == vendor_id and port.pid == product_id: - print(f"Found device on port: {port.device}") - return port.device + 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 +# Update the script to use a list of device IDs. +DEVICE_IDS = [ + (0x1A86, 0x55D3), # Example device 1 + # Add more (vendor_id, product_id) pairs here as needed +] + # Parse arguments. This allows an override if you supply --port. parser = argparse.ArgumentParser() parser.add_argument( @@ -38,8 +43,8 @@ args = parser.parse_args() # If no port is provided, try to auto-detect using the USB id info. if args.port is None: - auto_port = find_port(TARGET_VENDOR_ID, TARGET_PRODUCT_ID) - if auto_port is None: + auto_port = find_port(DEVICE_IDS) + if (auto_port is None): fallback_port = "/dev/ttyACM0" print(f"Device not auto-detected. Attempting connection using fallback port {fallback_port}.") args.port = fallback_port From 1180c69256f26b219ea7266b5ceb3cbebe123640 Mon Sep 17 00:00:00 2001 From: apirrone Date: Fri, 25 Apr 2025 17:18:09 +0200 Subject: [PATCH 3/5] improving and cleaning stuff --- mini_bdx_runtime/mini_bdx_runtime/utils.py | 51 ++++++++++++++ scripts/configure_motor.py | 81 ++++++---------------- setup.cfg | 1 + 3 files changed, 74 insertions(+), 59 deletions(-) create mode 100644 mini_bdx_runtime/mini_bdx_runtime/utils.py 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 2110d78..d62a73c 100644 --- a/scripts/configure_motor.py +++ b/scripts/configure_motor.py @@ -1,78 +1,41 @@ from pypot.feetech import FeetechSTS3215IO import argparse import time -import serial.tools.list_ports +from mini_bdx_runtime.utils import get_port +import tqdm DEFAULT_ID = 1 # A brand new motor should have id 1 -# These are placeholders. Please replace these with your actual device's USB IDs. -TARGET_VENDOR_ID = 0x1A86 # e.g., your device's vendor id (in hex) -TARGET_PRODUCT_ID = 0x55D3 # e.g., your device's product id (in hex) - -# Update the find_port function to accept a list of (vendor_id, product_id) tuples. -def find_port(device_ids): - """ - 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 - -# Update the script to use a list of device IDs. -DEVICE_IDS = [ - (0x1A86, 0x55D3), # Example device 1 - # Add more (vendor_id, product_id) pairs here as needed -] - # 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. If not specified, " - "the script will try to auto-detect the port using provided USB IDs or " - "fall back to /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() +port = args.port # If no port is provided, try to auto-detect using the USB id info. -if args.port is None: - auto_port = find_port(DEVICE_IDS) - if (auto_port is None): - fallback_port = "/dev/ttyACM0" - print(f"Device not auto-detected. Attempting connection using fallback port {fallback_port}.") - args.port = fallback_port - else: - args.port = auto_port +if port is None: + port = get_port() +if port is None: + exit() -# Attempt to connect and catch any connection errors. -try: - io = FeetechSTS3215IO(args.port) -except Exception as exc: - message = f"Error connecting to the motor using port {args.port}: {exc}. Please check your connection.\n" - # If the fallback port was used, advise the user with additional usage information. - fallback_port = "/dev/ttyACM0" - if args.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) - exit(1) + +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 @@ -90,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: @@ -97,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]) @@ -106,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}) @@ -123,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/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 From d16f47430c6d4cb22b760da3f7b1bb30b2fd4913 Mon Sep 17 00:00:00 2001 From: apirrone Date: Fri, 25 Apr 2025 17:24:02 +0200 Subject: [PATCH 4/5] serial port in duck_config --- example_config.json | 1 + mini_bdx_runtime/mini_bdx_runtime/duck_config.py | 1 + mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py | 3 ++- scripts/v2_rl_walk_mujoco.py | 3 +-- 4 files changed, 5 insertions(+), 3 deletions(-) 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..3bfaab3 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, usb_port: str = None): self.duck_config = duck_config @@ -74,6 +74,7 @@ class HWI: self.kds = np.ones(len(self.joints)) * 0 # default kd self.low_torque_kps = np.ones(len(self.joints)) * 2 + usb_port = usb_port if usb_port else self.duck_config.serial_port self.io = rustypot.feetech(usb_port, 1000000) def set_kps(self, kps): diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index d9e5f46..00dd10d 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/scripts/v2_rl_walk_mujoco.py @@ -27,7 +27,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, @@ -67,7 +66,7 @@ class RLWalk: self.control_freq, cutoff_frequency ) - self.hwi = HWI(self.duck_config, serial_port) + self.hwi = HWI(self.duck_config) self.start() From 32e1d1c3ac869512a0f701243cbfa3f33b65aee3 Mon Sep 17 00:00:00 2001 From: apirrone Date: Fri, 25 Apr 2025 17:36:50 +0200 Subject: [PATCH 5/5] doesn't make sense to keep the usb_port argument in HWI --- mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 3bfaab3..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 = None): + def __init__(self, duck_config: DuckConfig): self.duck_config = duck_config @@ -74,8 +74,7 @@ class HWI: self.kds = np.ones(len(self.joints)) * 0 # default kd self.low_torque_kps = np.ones(len(self.joints)) * 2 - usb_port = usb_port if usb_port else self.duck_config.serial_port - 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