From 1df95b3314add9dacdc905ef994585fde8b13de2 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 5 May 2026 17:52:10 +0200 Subject: [PATCH 01/10] Address PR #62 review feedback (Feetech client cleanup) - Drop redundant read_pos_vel_cur sync wrapper; the public method now does the sync read directly with per-motor fallback - Rename _read_*_individual fallbacks to _read_*_per_motor_fallback so the intent is obvious at the call site - Centralize POSITION_DIRECTION via _raw_to_rad / _rad_to_raw helpers so the sign inversion stops leaking into read/write logic - Define MotorError + MotionTimeoutError; wait_for_motion_complete now raises on timeout instead of silently returning False - Add MotorClient.waits_for_motion class flag; OrcaHand.wait_for_motion skips the motor lock when the underlying client doesn't actually block - Strip the verbose AI-style comment from calibrate_offset - Add explicit parens to the sign-flip in write_positions_sync - Rename scripts/test.py -> scripts/stress_test.py and update docs --- .../pages/orca-core-docs/orca-core-scripts.md | 10 +- orca_core/hardware/feetech_client.py | 73 +++--- orca_core/hardware/motor_client.py | 27 ++- orca_core/hardware_hand.py | 10 +- scripts/stress_test.py | 209 ++++++++++-------- scripts/test.py | 169 -------------- 6 files changed, 177 insertions(+), 321 deletions(-) delete mode 100644 scripts/test.py diff --git a/docs/pages/orca-core-docs/orca-core-scripts.md b/docs/pages/orca-core-docs/orca-core-scripts.md index ccb55b98..3acf9762 100644 --- a/docs/pages/orca-core-docs/orca-core-scripts.md +++ b/docs/pages/orca-core-docs/orca-core-scripts.md @@ -239,17 +239,21 @@ python scripts/tension.py /path/to/orcahand_v1_left/config.yaml
-test.py +stress_test.py -A test script that connects to the ORCA Hand, enables torque, sets a specific pose for a few joints (index_mcp, middle_pip, pinky_abd), waits for 2 seconds, disables torque, and disconnects. +Cycles the hand between OPEN and CLOSE poses while monitoring motor temperatures, aborting if any motor exceeds the safe operating temperature.
Args:
Example: ```bash -python scripts/test.py +python scripts/stress_test.py ```
diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index 199fb5e9..911ce395 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -14,7 +14,7 @@ from typing import Optional, Sequence, Tuple import numpy as np -from .motor_client import MotorClient +from .motor_client import MotionTimeoutError, MotorClient from .feetech import ( PortHandler, sms_sts, @@ -67,6 +67,8 @@ class FeetechClient(MotorClient): providing compatibility with the OrcaHand control system. """ + waits_for_motion = True + OPEN_CLIENTS = set() def __init__( @@ -249,16 +251,8 @@ def set_operating_mode(self, motor_ids: Sequence[int], mode: int) -> None: # Re-enable torque self.set_torque_enabled(motor_ids, True) - def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Reads position, velocity, and current for all motors. - - Tries sync read first (one packet for all motors); falls back to - per-motor reads only if the sync transaction fails. - """ - return self.read_pos_vel_cur_sync() - - def _read_pos_vel_cur_individual(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Per-motor fallback used only when sync read fails.""" + def _read_state_per_motor_fallback(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + """Per-motor read of position/velocity/current; used only when sync read fails.""" self._check_connected() positions = np.zeros(len(self.motor_ids), dtype=np.float32) @@ -273,7 +267,7 @@ def _read_pos_vel_cur_individual(self) -> Tuple[np.ndarray, np.ndarray, np.ndarr if result == COMM_SUCCESS and error == 0: pos_signed = self.packet_handler.scs_tohost(pos_raw, 15) pos_normalized = self._normalize_position(pos_signed) - positions[i] = pos_normalized * self.pos_scale * POSITION_DIRECTION + positions[i] = self._raw_to_rad(pos_normalized, self.pos_scale) else: logging.warning( 'Failed to read position for motor %d: result=%d, error=%d', @@ -286,7 +280,7 @@ def _read_pos_vel_cur_individual(self) -> Tuple[np.ndarray, np.ndarray, np.ndarr ) if result == COMM_SUCCESS and error == 0: vel_signed = self.packet_handler.scs_tohost(vel_raw, 15) - velocities[i] = vel_signed * self.vel_scale * POSITION_DIRECTION + velocities[i] = self._raw_to_rad(vel_signed, self.vel_scale) else: logging.warning( 'Failed to read velocity for motor %d: result=%d, error=%d', @@ -320,7 +314,7 @@ def read_temperature(self) -> np.ndarray: if sync_read.txRxPacket() != COMM_SUCCESS: logging.warning('Sync temp read failed, falling back to individual reads') - return self._read_temperature_individual() + return self._read_temperature_per_motor_fallback() for i, motor_id in enumerate(self.motor_ids): available, _ = sync_read.isAvailable(motor_id, SMS_STS_PRESENT_TEMPERATURE, 1) @@ -331,8 +325,8 @@ def read_temperature(self) -> np.ndarray: return temperatures - def _read_temperature_individual(self) -> np.ndarray: - """Per-motor fallback used only when sync temp read fails.""" + def _read_temperature_per_motor_fallback(self) -> np.ndarray: + """Per-motor read of temperature; used only when sync read fails.""" temperatures = np.zeros(len(self.motor_ids), dtype=np.float32) for i, motor_id in enumerate(self.motor_ids): temp, result, error = self.packet_handler.read1ByteTxRx( @@ -352,8 +346,8 @@ def wait_for_motion_complete( self, timeout: float = 5.0, poll_interval: float = 0.02, - ) -> bool: - """Block until every motor reports it has stopped, or timeout elapses. + ) -> None: + """Block until every motor reports it has stopped. Reads each motor's MOVING flag (register 66) via sync read; the bit is 1 while the servo is travelling toward its goal position and 0 @@ -363,8 +357,9 @@ def wait_for_motion_complete( timeout: Max seconds to wait before giving up. poll_interval: Seconds between polls. - Returns: - True if all motors stopped within the timeout, False otherwise. + Raises: + MotionTimeoutError: If any motor is still moving when the + timeout elapses. """ self._check_connected() deadline = time.monotonic() + timeout @@ -388,11 +383,12 @@ def wait_for_motion_complete( break if all_stopped: - return True + return time.sleep(poll_interval) - logging.warning('wait_for_motion_complete: timed out after %.1fs', timeout) - return False + raise MotionTimeoutError( + f'Motors did not settle within {timeout:.1f}s' + ) def write_desired_pos( self, @@ -465,6 +461,16 @@ def _clamp_position(self, pos_raw: int) -> int: """Clamp position to valid servo range (0-4095).""" return max(POS_MIN, min(POS_MAX, pos_raw)) + @staticmethod + def _raw_to_rad(raw: float, scale: float) -> float: + """Convert raw motor units to radians (applies direction inversion).""" + return raw * scale * POSITION_DIRECTION + + @staticmethod + def _rad_to_raw(rad: float, scale: float) -> int: + """Convert radians to raw motor units (applies direction inversion).""" + return int((rad * POSITION_DIRECTION) / scale) + @property def requires_offset_calibration(self) -> bool: return True @@ -484,14 +490,10 @@ def calibrate_offset(self, motor_id: int, upper: bool = True) -> bool: """ self._check_connected() - # OrcaHand's "upper" means the FLEX-side limit in its read frame. With - # POSITION_DIRECTION = -1 that maps to LOW raw (since raw → -raw·scale), - # so we flip which raw end gets the buffer; otherwise the next-direction - # motion saturates against the encoder boundary after only ~44°. + # When POSITION_DIRECTION inverts the read frame, "upper" maps to low raw. if POSITION_DIRECTION < 0: upper = not upper - # Buffer of 500 (~44°) ensures room for tendon loosening over time target_position = 3595 if upper else 500 result, error = self.packet_handler.reOfsCal(motor_id, target_position) @@ -557,8 +559,7 @@ def write_positions_sync( self.packet_handler.groupSyncWrite.clearParam() for motor_id, pos_rad in zip(motor_ids, positions): - pos_raw = int(pos_rad * POSITION_DIRECTION / self.pos_scale) - pos_raw = self._clamp_position(pos_raw) + pos_raw = self._clamp_position(self._rad_to_raw(pos_rad, self.pos_scale)) logging.debug( 'SyncWritePosEx: motor=%d, pos=%d, speed=%d, acc=%d, torque=%d', @@ -575,10 +576,10 @@ def write_positions_sync( # Clear params for next use self.packet_handler.groupSyncWrite.clearParam() - def read_pos_vel_cur_sync(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Reads position, velocity, and current for all motors using sync read. + def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + """Read position, velocity, and current for all motors in one sync packet. - More efficient than individual reads for multiple motors. + Falls back to per-motor reads only if the sync transaction fails. """ self._check_connected() @@ -596,7 +597,7 @@ def read_pos_vel_cur_sync(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: result = sync_read.txRxPacket() if result != COMM_SUCCESS: logging.warning('Sync read failed, falling back to individual reads') - return self._read_pos_vel_cur_individual() + return self._read_state_per_motor_fallback() for i, motor_id in enumerate(self.motor_ids): available, error = sync_read.isAvailable( @@ -606,11 +607,11 @@ def read_pos_vel_cur_sync(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: pos_raw = sync_read.getData(motor_id, SMS_STS_PRESENT_POSITION_L, 2) pos_signed = self.packet_handler.scs_tohost(pos_raw, 15) pos_normalized = self._normalize_position(pos_signed) - positions[i] = pos_normalized * self.pos_scale * POSITION_DIRECTION + positions[i] = self._raw_to_rad(pos_normalized, self.pos_scale) vel_raw = sync_read.getData(motor_id, SMS_STS_PRESENT_SPEED_L, 2) vel_signed = self.packet_handler.scs_tohost(vel_raw, 15) - velocities[i] = vel_signed * self.vel_scale * POSITION_DIRECTION + velocities[i] = self._raw_to_rad(vel_signed, self.vel_scale) cur_raw = sync_read.getData(motor_id, SMS_STS_PRESENT_CURRENT_L, 2) cur_signed = self.packet_handler.scs_tohost(cur_raw, 15) diff --git a/orca_core/hardware/motor_client.py b/orca_core/hardware/motor_client.py index 27bdc0a4..2986e1b1 100644 --- a/orca_core/hardware/motor_client.py +++ b/orca_core/hardware/motor_client.py @@ -13,6 +13,14 @@ import numpy as np +class MotorError(Exception): + """Raised when a motor operation cannot be completed.""" + + +class MotionTimeoutError(MotorError): + """Raised when motors fail to settle within the requested timeout.""" + + class MotorClient(ABC): """Abstract base class for motor communication clients. @@ -20,6 +28,10 @@ class MotorClient(ABC): must implement to work with OrcaHand. """ + # Subclasses set this to True when ``wait_for_motion_complete`` actually + # blocks; callers can use it to skip locking around no-op waits. + waits_for_motion: bool = False + @property @abstractmethod def is_connected(self) -> bool: @@ -116,22 +128,15 @@ def write_desired_current( """ ... - def wait_for_motion_complete(self, timeout: float = 5.0) -> bool: + def wait_for_motion_complete(self, timeout: float = 5.0) -> None: """Block until all motors finish their commanded motion. Default implementation is a no-op for motor families that respond fast enough that callers don't need to wait (e.g., Dynamixel, mock). - Slower buses like Feetech override this to poll a per-motor moving - flag. - - Args: - timeout: Max seconds to wait. - - Returns: - True if motion completed (or no waiting was needed), False on - timeout. + Subclasses that actually block (e.g., Feetech) must set + ``waits_for_motion = True`` and override this to poll a per-motor + moving flag, raising ``MotionTimeoutError`` on timeout. """ - return True @property def requires_offset_calibration(self) -> bool: diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 11f67f73..2ff17d70 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -366,7 +366,7 @@ def get_motor_current(self, as_dict: bool = False) -> Union[np.ndarray, dict]: return motor_current - def wait_for_motion(self, timeout: float = 5.0) -> bool: + def wait_for_motion(self, timeout: float = 5.0) -> None: """Block until all motors have settled at their commanded position. No-op for motor types fast enough that callers don't need to wait @@ -375,11 +375,13 @@ def wait_for_motion(self, timeout: float = 5.0) -> bool: Args: timeout: Max seconds to wait. - Returns: - True if all motors stopped within the timeout, False otherwise. + Raises: + MotionTimeoutError: If motors fail to settle within ``timeout``. """ + if not self._motor_client.waits_for_motion: + return with self._motor_lock: - return self._motor_client.wait_for_motion_complete(timeout=timeout) + self._motor_client.wait_for_motion_complete(timeout=timeout) def get_motor_temp(self, as_dict: bool = False) -> Union[np.ndarray, dict]: """Read the present temperature of each motor. diff --git a/scripts/stress_test.py b/scripts/stress_test.py index cbfdf8bb..7978e2c4 100644 --- a/scripts/stress_test.py +++ b/scripts/stress_test.py @@ -1,14 +1,14 @@ +"""Cycle the hand between OPEN and CLOSE poses while monitoring motor temperatures.""" - -import time import argparse +import time from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand -from orca_core import OrcaJointPositions -from orca_core.constants import NUM_STEPS, STEP_SIZE -# Max operating temp (°C) — conservative across XC330 (70°C) and XC430 (72°C) -MAX_TEMP = 70 +from orca_core.hardware.motor_client import MotionTimeoutError + + +MAX_TEMP = 70 # °C — conservative across Dynamixel XC330/430 and Feetech STS3215 TEMP_CHECK_INTERVAL = 2.0 @@ -20,138 +20,151 @@ DIM = "\033[2m" -def temp_color(pct): +def temp_color(pct: float) -> str: if pct >= 90: return RED - elif pct >= 70: + if pct >= 70: return YELLOW return GREEN -def print_temp_table(hand, temps): +def print_temp_table(hand, temps: dict) -> None: """Print a compact color-coded temperature table grouped by finger.""" motor_to_joint = hand.config.motor_to_joint_dict - # Group motors by finger - fingers = ['thumb', 'index', 'middle', 'ring', 'pinky', 'wrist'] + fingers = ["thumb", "index", "middle", "ring", "pinky", "wrist"] grouped = {f: [] for f in fingers} for mid, t in temps.items(): joint = motor_to_joint.get(mid, f"motor_{mid}") - finger = joint.split('_')[0] + finger = joint.split("_")[0] if finger in grouped: grouped[finger].append((joint, mid, t)) - # Clear screen and move cursor to top - print("\033[2J\033[H", end="") + print("\033[2J\033[H", end="") # clear screen, cursor home print(f"{BOLD} ORCA Hand Temperature Monitor{RST}") print(f" {DIM}Max operating temp: {MAX_TEMP}°C{RST}\n") - - # Header print(f" {BOLD}{'Joint':<14} {'Motor':>5} {'Temp':>6} {'%Max':>6} {'':>10}{RST}") print(f" {'─' * 48}") for finger in fingers: - joints = grouped[finger] - if not joints: - continue - for joint, mid, t in joints: + for joint, mid, t in grouped[finger]: pct = t / MAX_TEMP * 100 c = temp_color(pct) - bar_len = int(pct / 100 * 10) + bar_len = int(min(pct, 100) / 100 * 10) bar = f"{c}{'█' * bar_len}{DIM}{'░' * (10 - bar_len)}{RST}" print(f" {joint:<14} {mid:>5} {c}{t:>4.0f}°C {pct:>5.0f}%{RST} {bar}") print(f" {'─' * 48}") - max_t = max(temps.values()) - max_pct = max_t / MAX_TEMP * 100 - c = temp_color(max_pct) - print(f" {'Peak':<14} {'':>5} {c}{max_t:>4.0f}°C {max_pct:>5.0f}%{RST}\n") - - -def pose_from_fractions(hand, fractions: dict[str, float]) -> OrcaJointPositions: - pose = dict(hand.config.neutral_position) - for joint, fraction in fractions.items(): - if joint not in hand.config.joint_roms_dict: - continue - joint_min, joint_max = hand.config.joint_roms_dict[joint] - pose[joint] = joint_min + fraction * (joint_max - joint_min) - return OrcaJointPositions.from_dict(pose) + if temps: + max_t = max(temps.values()) + max_pct = max_t / MAX_TEMP * 100 + c = temp_color(max_pct) + print(f" {'Peak':<14} {'':>5} {c}{max_t:>4.0f}°C {max_pct:>5.0f}%{RST}\n") + + +JOINT_OPEN = { + "index_abd": -30, + "middle_abd": 0, + "ring_abd": 10, + "pinky_abd": 25, + "thumb_abd": -20, + "index_mcp": -40, + "middle_mcp": -40, + "ring_mcp": -40, + "pinky_mcp": -40, + "thumb_mcp": 45, + "index_pip": -10, + "middle_pip": -10, + "ring_pip": -10, + "pinky_pip": -10, + "thumb_dip": 90, + "thumb_cmc": 40, + "wrist": -25, +} + +JOINT_CLOSE = { + "index_abd": 15, + "middle_abd": 15, + "ring_abd": -15, + "pinky_abd": -15, + "thumb_abd": 55, + "index_mcp": 45, + "middle_mcp": 45, + "ring_mcp": 45, + "pinky_mcp": 45, + "thumb_mcp": -40, + "index_pip": 90, + "middle_pip": 90, + "ring_pip": 90, + "pinky_pip": 90, + "thumb_dip": -30, + "thumb_cmc": -50, + "wrist": 10, +} def main() -> int: parser = argparse.ArgumentParser( - description="Run a looping temperature monitor plus open/close hand demo." + description="Open/close cycle stress test with temperature monitoring." ) add_hand_arguments(parser) - parser.add_argument("--cycles", type=int, default=1, help="Number of open/close cycles. Use 0 for infinite.") + parser.add_argument( + "--num-steps", type=int, default=25, + help="Interpolation steps per move (default 25)." + ) + parser.add_argument( + "--step-size", type=float, default=0.001, + help="Sleep between interpolation steps in seconds (default 0.001)." + ) + parser.add_argument( + "--hold", type=float, default=0.0, + help="Extra seconds to hold each pose AFTER motion completes (default 0)." + ) + parser.add_argument( + "--motion-timeout", type=float, default=5.0, + help="Max seconds to wait for a pose to be reached (default 5)." + ) args = parser.parse_args() hand = create_hand(args.config_path, use_mock=args.mock) try: connect_hand(hand) - hand.init_joints(force_calibrate=args.mock) - - open_pose = pose_from_fractions( - hand, - { - "thumb_cmc": 0.70, - "thumb_abd": 0.80, - "thumb_mcp": 0.85, - "thumb_dip": 0.80, - "index_abd": 0.10, - "middle_abd": 0.50, - "ring_abd": 0.70, - "pinky_abd": 0.85, - "index_mcp": 0.15, - "middle_mcp": 0.15, - "ring_mcp": 0.15, - "pinky_mcp": 0.15, - "index_pip": 0.10, - "middle_pip": 0.10, - "ring_pip": 0.10, - "pinky_pip": 0.10, - "wrist": 0.25, - }, - ) - close_pose = pose_from_fractions( - hand, - { - "thumb_cmc": 0.35, - "thumb_abd": 0.55, - "thumb_mcp": 0.20, - "thumb_dip": 0.90, - "index_mcp": 0.85, - "middle_mcp": 0.85, - "ring_mcp": 0.85, - "pinky_mcp": 0.85, - "index_pip": 0.90, - "middle_pip": 0.90, - "ring_pip": 0.90, - "pinky_pip": 0.90, - "wrist": 0.55, - }, - ) - - cycles_remaining = args.cycles - last_temp_check = 0.0 - while cycles_remaining != 0: - now = time.monotonic() - if now - last_temp_check >= TEMP_CHECK_INTERVAL: - last_temp_check = now - print_temp_table(hand, hand.get_motor_temp(as_dict=True)) + hand.enable_torque() - hand.set_joint_positions(open_pose, num_steps=NUM_STEPS, step_size=STEP_SIZE) - time.sleep(2.0) - hand.set_joint_positions(close_pose, num_steps=NUM_STEPS, step_size=STEP_SIZE) - time.sleep(2.0) - - if cycles_remaining > 0: - cycles_remaining -= 1 - - return 0 - except KeyboardInterrupt: - print("\nDemo interrupted.") + last_temp_check = 0.0 + try: + while True: + now = time.monotonic() + if now - last_temp_check >= TEMP_CHECK_INTERVAL: + last_temp_check = now + temps = hand.get_motor_temp(as_dict=True) + print_temp_table(hand, temps) + if temps and max(temps.values()) >= MAX_TEMP: + print(f"{RED}Motor temperature reached {MAX_TEMP}°C — aborting.{RST}") + break + + hand.set_joint_positions( + JOINT_OPEN, num_steps=args.num_steps, step_size=args.step_size + ) + try: + hand.wait_for_motion(timeout=args.motion_timeout) + except MotionTimeoutError as exc: + print(f"{YELLOW}Motion timed out: {exc}{RST}") + if args.hold: + time.sleep(args.hold) + + hand.set_joint_positions( + JOINT_CLOSE, num_steps=args.num_steps, step_size=args.step_size + ) + try: + hand.wait_for_motion(timeout=args.motion_timeout) + except MotionTimeoutError as exc: + print(f"{YELLOW}Motion timed out: {exc}{RST}") + if args.hold: + time.sleep(args.hold) + except KeyboardInterrupt: + print("\nInterrupted.") return 0 finally: shutdown_hand(hand) diff --git a/scripts/test.py b/scripts/test.py deleted file mode 100644 index 8bd56be9..00000000 --- a/scripts/test.py +++ /dev/null @@ -1,169 +0,0 @@ -"""Cycle the hand between OPEN and CLOSE poses while monitoring motor temperatures. - -Adapted from the fb/dev test.py for the current OrcaHand API. -""" - -import argparse -import time - -from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand - - -MAX_TEMP = 70 # °C — conservative across Dynamixel XC330/430 and Feetech STS3215 -TEMP_CHECK_INTERVAL = 2.0 - - -RST = "\033[0m" -GREEN = "\033[92m" -YELLOW = "\033[93m" -RED = "\033[91m" -BOLD = "\033[1m" -DIM = "\033[2m" - - -def temp_color(pct: float) -> str: - if pct >= 90: - return RED - if pct >= 70: - return YELLOW - return GREEN - - -def print_temp_table(hand, temps: dict) -> None: - """Print a compact color-coded temperature table grouped by finger.""" - motor_to_joint = hand.config.motor_to_joint_dict - - fingers = ["thumb", "index", "middle", "ring", "pinky", "wrist"] - grouped = {f: [] for f in fingers} - for mid, t in temps.items(): - joint = motor_to_joint.get(mid, f"motor_{mid}") - finger = joint.split("_")[0] - if finger in grouped: - grouped[finger].append((joint, mid, t)) - - print("\033[2J\033[H", end="") # clear screen, cursor home - - print(f"{BOLD} ORCA Hand Temperature Monitor{RST}") - print(f" {DIM}Max operating temp: {MAX_TEMP}°C{RST}\n") - print(f" {BOLD}{'Joint':<14} {'Motor':>5} {'Temp':>6} {'%Max':>6} {'':>10}{RST}") - print(f" {'─' * 48}") - - for finger in fingers: - for joint, mid, t in grouped[finger]: - pct = t / MAX_TEMP * 100 - c = temp_color(pct) - bar_len = int(min(pct, 100) / 100 * 10) - bar = f"{c}{'█' * bar_len}{DIM}{'░' * (10 - bar_len)}{RST}" - print(f" {joint:<14} {mid:>5} {c}{t:>4.0f}°C {pct:>5.0f}%{RST} {bar}") - - print(f" {'─' * 48}") - if temps: - max_t = max(temps.values()) - max_pct = max_t / MAX_TEMP * 100 - c = temp_color(max_pct) - print(f" {'Peak':<14} {'':>5} {c}{max_t:>4.0f}°C {max_pct:>5.0f}%{RST}\n") - - -JOINT_OPEN = { - "index_abd": -30, - "middle_abd": 0, - "ring_abd": 10, - "pinky_abd": 25, - "thumb_abd": -20, - "index_mcp": -40, - "middle_mcp": -40, - "ring_mcp": -40, - "pinky_mcp": -40, - "thumb_mcp": 45, - "index_pip": -10, - "middle_pip": -10, - "ring_pip": -10, - "pinky_pip": -10, - "thumb_dip": 90, - "thumb_cmc": 40, - "wrist": -25, -} - -JOINT_CLOSE = { - "index_abd": 15, - "middle_abd": 15, - "ring_abd": -15, - "pinky_abd": -15, - "thumb_abd": 55, - "index_mcp": 45, - "middle_mcp": 45, - "ring_mcp": 45, - "pinky_mcp": 45, - "thumb_mcp": -40, - "index_pip": 90, - "middle_pip": 90, - "ring_pip": 90, - "pinky_pip": 90, - "thumb_dip": -30, - "thumb_cmc": -50, - "wrist": 10, -} - - -def main() -> int: - parser = argparse.ArgumentParser( - description="Open/close cycle test with temperature monitoring." - ) - add_hand_arguments(parser) - parser.add_argument( - "--num-steps", type=int, default=25, - help="Interpolation steps per move (default 25)." - ) - parser.add_argument( - "--step-size", type=float, default=0.001, - help="Sleep between interpolation steps in seconds (default 0.001)." - ) - parser.add_argument( - "--hold", type=float, default=0.0, - help="Extra seconds to hold each pose AFTER motion completes (default 0)." - ) - parser.add_argument( - "--motion-timeout", type=float, default=5.0, - help="Max seconds to wait for a pose to be reached (default 5)." - ) - args = parser.parse_args() - - hand = create_hand(args.config_path, use_mock=args.mock) - try: - connect_hand(hand) - hand.enable_torque() - - last_temp_check = 0.0 - try: - while True: - now = time.monotonic() - if now - last_temp_check >= TEMP_CHECK_INTERVAL: - last_temp_check = now - temps = hand.get_motor_temp(as_dict=True) - print_temp_table(hand, temps) - if temps and max(temps.values()) >= MAX_TEMP: - print(f"{RED}Motor temperature reached {MAX_TEMP}°C — aborting.{RST}") - break - - hand.set_joint_positions( - JOINT_OPEN, num_steps=args.num_steps, step_size=args.step_size - ) - hand.wait_for_motion(timeout=args.motion_timeout) - if args.hold: - time.sleep(args.hold) - - hand.set_joint_positions( - JOINT_CLOSE, num_steps=args.num_steps, step_size=args.step_size - ) - hand.wait_for_motion(timeout=args.motion_timeout) - if args.hold: - time.sleep(args.hold) - except KeyboardInterrupt: - print("\nInterrupted.") - return 0 - finally: - shutdown_hand(hand) - - -if __name__ == "__main__": - raise SystemExit(main()) From b5e55d02734354f03a5daddb009fad51fbb28249 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Wed, 6 May 2026 14:33:59 +0200 Subject: [PATCH 02/10] Auto-detect motor driver and baudrate at connect time MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Configs no longer need to declare motor_type, port, or baudrate. At connect time the client probes attached USB serial adapters and pings each candidate (motor_type, baudrate) pair until one responds. - Add probe() static method on DynamixelClient and FeetechClient that opens a port at a given baudrate, sends a few pings, and reports whether any motor responds — without enabling torque - Add MOTOR_BAUD_RATES (constants.py): {dynamixel: [1M, 3M], feetech: [1M]} - Add motor_type_for_port and find_single_usb_serial_port helpers in utils for VID-based detection and lone-adapter fallback - OrcaHandConfig.motor_type/port/baudrate become Optional; explicit values still win and short-circuit the probe along that axis - OrcaHand.connect now does: 1. resolve port (yaml > VID > sole USB adapter > interactive picker) 2. trial-probe to find motor family + baudrate 3. instantiate the right client and connect An auto-detected port is persisted to config.yaml; motor_type and baudrate are intentionally not persisted so configs stay driver-agnostic - Strip motor_type/port/baudrate from all bundled v1/v2 model configs - Delete orcahand_right_feetech model dir; orcahand_right is now used for both Dynamixel and Feetech builds of the same hand - Update stress_test.py defaults: --num-steps from constants (50), --step-size from constants (0.01s), --hold default 2s — restoring pre-rename motion timing instead of the way-too-fast 25 steps × 1ms - Add tests/test_connect_resolution.py covering port lookup, single-USB fallback, trial-probe at multiple baudrates, and pinned-motor_type --- orca_core/constants.py | 8 + orca_core/hand_config.py | 26 ++- orca_core/hardware/dynamixel_client.py | 33 +++ orca_core/hardware/feetech_client.py | 30 +++ orca_core/hardware_hand.py | 180 +++++++++++----- orca_core/models/v1/orcahand_left/config.yaml | 2 - .../models/v1/orcahand_right/config.yaml | 3 - orca_core/models/v2/orcahand_left/config.yaml | 2 - .../models/v2/orcahand_right/config.yaml | 2 - .../v2/orcahand_right_feetech/config.yaml | 203 ------------------ .../models/v2/orcahand_touch_left/config.yaml | 2 - .../v2/orcahand_touch_right/config.yaml | 2 - orca_core/utils/utils.py | 30 +++ scripts/stress_test.py | 26 +-- tests/test_connect_resolution.py | 144 +++++++++++++ 15 files changed, 396 insertions(+), 297 deletions(-) delete mode 100644 orca_core/models/v2/orcahand_right_feetech/config.yaml create mode 100644 tests/test_connect_resolution.py diff --git a/orca_core/constants.py b/orca_core/constants.py index b023eab9..b9c3dbbc 100644 --- a/orca_core/constants.py +++ b/orca_core/constants.py @@ -22,6 +22,14 @@ ], } +# Baudrates the connect-time probe will try, per motor family, in priority +# order. Used only when ``baudrate`` is not pinned in config.yaml. Feetech +# motors ship at 1M; Dynamixels run at 1M (v2 hands) or 3M (v1 hands). +MOTOR_BAUD_RATES: dict[str, list[int]] = { + "dynamixel": [1_000_000, 3_000_000], + "feetech": [1_000_000], +} + """ Dynamixel specific! TODO(fracapuano): Add feetech control modes too. PWM (id: 2) control modeis omitted becayse it bypasses PID controllers entirely. diff --git a/orca_core/hand_config.py b/orca_core/hand_config.py index ab0ac2ca..4662720a 100644 --- a/orca_core/hand_config.py +++ b/orca_core/hand_config.py @@ -8,9 +8,17 @@ import os from dataclasses import dataclass, field -from typing import Dict, List, Literal - -from .constants import CONTROL_MODES, DEFAULT_MODEL_NAME, JOINT_IDS, JOINT_ROM_DICT, JOINT_TO_MOTOR_MAP, MOTOR_IDS +from typing import Dict, List, Literal, Optional + +from .constants import ( + CONTROL_MODES, + DEFAULT_MODEL_NAME, + JOINT_IDS, + JOINT_ROM_DICT, + JOINT_TO_MOTOR_MAP, + MOTOR_IDS, + SUPPORTED_MOTOR_TYPES, +) from .joint_position import OrcaJointPositions from .utils.utils import get_model_path, read_yaml @@ -181,11 +189,11 @@ class OrcaHandConfig(BaseHandConfig): """ORCA hand configuration layered on top of the shared base spec.""" calibration_path: str = "" - baudrate: int = 3_000_000 - port: str = "/dev/ttyUSB0" + baudrate: Optional[int] = None + port: Optional[str] = None max_current: int = 300 # mA control_mode: str = "current_based_position" - motor_type: str = "dynamixel" + motor_type: Optional[str] = None motor_ids: List[int] = field(default_factory=list) joint_to_motor_map: Dict[str, int] = field(default_factory=dict) joint_inversion_dict: Dict[str, bool] = field(default_factory=dict) @@ -291,6 +299,12 @@ def validate_config(self) -> None: if self.control_mode not in CONTROL_MODES: raise HandConfigValidationError("Invalid control mode.") + if self.motor_type is not None and self.motor_type not in SUPPORTED_MOTOR_TYPES: + raise HandConfigValidationError( + f"Unknown motor_type: {self.motor_type!r}. " + f"Expected one of {SUPPORTED_MOTOR_TYPES} or omit for auto-detection." + ) + if self.max_current < self.calibration_current: raise HandConfigValidationError( "Max current should be greater than the calibration current." diff --git a/orca_core/hardware/dynamixel_client.py b/orca_core/hardware/dynamixel_client.py index e89a066e..337740c5 100644 --- a/orca_core/hardware/dynamixel_client.py +++ b/orca_core/hardware/dynamixel_client.py @@ -214,6 +214,39 @@ def connect(self): # Start with all motors enabled. self.set_torque_enabled(self.motor_ids, True) + @staticmethod + def probe( + port: str, + baudrate: int, + motor_ids: Sequence[int], + ping_count: int = 2, + ) -> bool: + """Open ``port`` at ``baudrate`` and ping the first few motor IDs. + + Returns True if any motor responds — i.e. the bus is speaking the + Dynamixel Protocol 2.0 at this baudrate. Used at connect time to + auto-detect the driver family without enabling torque. + """ + from dynamixel_sdk import PortHandler, PacketHandler, COMM_SUCCESS + + handler = PortHandler(port) + try: + if not handler.openPort(): + return False + if not handler.setBaudRate(baudrate): + return False + packet = PacketHandler(PROTOCOL_VERSION) + for motor_id in list(motor_ids)[:ping_count]: + _, comm, _ = packet.ping(handler, motor_id) + if comm == COMM_SUCCESS: + return True + return False + finally: + try: + handler.closePort() + except Exception: + pass + def disconnect(self): """Disconnects from the Dynamixel device.""" if not self.is_connected: diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index 911ce395..c9f87e71 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -510,6 +510,36 @@ def calibrate_offset(self, motor_id: int, upper: bool = True) -> bool: ) return True + @staticmethod + def probe( + port: str, + baudrate: int, + motor_ids: Sequence[int], + ping_count: int = 2, + ) -> bool: + """Open ``port`` at ``baudrate`` and ping the first few motor IDs. + + Returns True if any motor responds — i.e. the bus is speaking the + Feetech protocol at this baudrate. Used at connect time to + auto-detect the driver family without enabling torque. + """ + handler = PortHandler(port) + handler.baudrate = baudrate + try: + if not handler.openPort(): + return False + packet = sms_sts(handler) + for motor_id in list(motor_ids)[:ping_count]: + _, comm, _ = packet.ping(motor_id) + if comm == COMM_SUCCESS: + return True + return False + finally: + try: + handler.closePort() + except Exception: + pass + def __enter__(self): """Enables use as a context manager.""" if not self._connected: diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 2ff17d70..e59b6b93 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -8,6 +8,7 @@ import dataclasses import math +import os import threading import time from collections import deque @@ -20,7 +21,13 @@ from .calibration import CalibrationResult from .hand_config import OrcaHandConfig from .hardware.motor_client import MotorClient -from .utils.utils import auto_detect_port, get_and_choose_port, update_yaml +from .utils.utils import ( + auto_detect_port, + find_single_usb_serial_port, + get_and_choose_port, + motor_type_for_port, + update_yaml, +) if TYPE_CHECKING: from .hardware.dynamixel_client import DynamixelClient @@ -28,6 +35,7 @@ from .constants import ( SUPPORTED_MOTOR_TYPES, + MOTOR_BAUD_RATES, MODE_MAP, WRIST_MODE_VALUE, CURRENT_BASED_POSITION, @@ -127,82 +135,132 @@ def calibrated(self) -> bool: def wrist_calibrated(self) -> bool: return self.calibration.wrist_calibrated - def _create_motor_client(self) -> MotorClient: - if self.config.motor_type == "dynamixel": + def _create_motor_client( + self, + motor_type: str, + port: str, + baudrate: int, + ) -> MotorClient: + if motor_type == "dynamixel": from .hardware.dynamixel_client import DynamixelClient - return DynamixelClient( - self.config.motor_ids, self.config.port, self.config.baudrate - ) + return DynamixelClient(self.config.motor_ids, port, baudrate) - if self.config.motor_type == "feetech": + if motor_type == "feetech": from .hardware.feetech_client import FeetechClient - return FeetechClient( - self.config.motor_ids, self.config.port, self.config.baudrate - ) + return FeetechClient(self.config.motor_ids, port, baudrate) raise ValueError( - f"Unknown motor_type: {self.config.motor_type}. Expected one of [{', '.join(SUPPORTED_MOTOR_TYPES)}]." + f"Unknown motor_type: {motor_type}. " + f"Expected one of [{', '.join(SUPPORTED_MOTOR_TYPES)}]." + ) + + def _resolve_port(self) -> str | None: + """Pick the serial port to talk to: explicit yaml > VID match > sole adapter.""" + if self.config.port and os.path.exists(self.config.port): + return self.config.port + if self.config.port: + print( + f"Configured port {self.config.port} not present; " + "falling back to USB auto-detection." + ) + # No yaml port (or the configured one is gone). Try VID-based detection + # for any motor family, then fall back to "the only adapter present". + for motor_type in SUPPORTED_MOTOR_TYPES: + port = auto_detect_port(motor_type) + if port: + return port + return find_single_usb_serial_port() + + def _trial_probe(self, port: str) -> tuple[str | None, int | None]: + """Probe ``port`` until a (motor_type, baudrate) combination responds. + + Iterates each motor family × the baudrates listed in + :data:`~orca_core.constants.MOTOR_BAUD_RATES`. If ``motor_type`` or + ``baudrate`` is pinned in yaml, that dimension is fixed and the + probe only iterates the other. + """ + from .hardware.dynamixel_client import DynamixelClient + from .hardware.feetech_client import FeetechClient + + candidates = { + "dynamixel": DynamixelClient, + "feetech": FeetechClient, + } + motor_types = ( + [self.config.motor_type] + if self.config.motor_type + else list(SUPPORTED_MOTOR_TYPES) ) + for motor_type in motor_types: + client_cls = candidates.get(motor_type) + if client_cls is None: + continue + baudrates = ( + [self.config.baudrate] + if self.config.baudrate is not None + else list(MOTOR_BAUD_RATES.get(motor_type, [])) + ) + for baudrate in baudrates: + print(f"Probing {motor_type} on {port} @ {baudrate} baud...") + try: + if client_cls.probe(port, baudrate, self.config.motor_ids): + print(f" -> {motor_type} responded at {baudrate} baud.") + return motor_type, baudrate + except Exception as e: + print(f" -> probe errored: {e}") + return None, None def connect(self) -> tuple[bool, str]: """Open connection to the motor bus. - Attempts to connect on the port in ``config.yaml``. On failure it - tries auto-detection via USB vendor ID, then falls back to an - interactive terminal port picker. A successful connection updates - ``config.yaml`` if the port changed. + Resolves (motor_type, port, baudrate) at connect time: + + 1. Find a port: explicit ``port`` in yaml wins; else VID-based + auto-detection; else the lone attached USB serial adapter; else + an interactive picker. + 2. Find the motor family / baudrate by pinging each candidate from + :data:`~orca_core.constants.MOTOR_BAUD_RATES` until one responds. + Explicit ``motor_type``/``baudrate`` in yaml short-circuit the + corresponding axis. Returns: A ``(success, message)`` tuple where *success* is ``True`` on a successful connection. """ - # TODO(fracapuano): Refactor: this is basically always connecting to one port and looking at multiple ports + port = self._resolve_port() + if port is None: + print("Please select a port from available devices:") + port = get_and_choose_port() + if port is None: + return False, "Connection failed: No port selected" + + motor_type, baudrate = self._trial_probe(port) + if motor_type is None or baudrate is None: + return False, ( + f"Connection failed: no motor responded on {port}. " + "Check power and wiring." + ) + try: - self._motor_client = self._create_motor_client() + self._motor_client = self._create_motor_client(motor_type, port, baudrate) with self._motor_lock: self._motor_client.connect() - return True, "Connection successful" + self._persist_resolved_port(port) + return True, f"Connection successful ({motor_type} @ {port}, {baudrate} baud)" except Exception as e: - # 1. The port is not correct self._motor_client = None - print(f"Connection failed on {self.config.port}: {str(e)}") - - chosen_port = auto_detect_port(self.config.motor_type) - if chosen_port and chosen_port != self.config.port: - # TODO(fracapuano): Replace replace replace this try except Exception is madness - try: - self.config = dataclasses.replace(self.config, port=chosen_port) - self._motor_client = self._create_motor_client() - with self._motor_lock: - self._motor_client.connect() - update_yaml(self.config.config_path, "port", chosen_port) - return ( - True, - f"Connection successful with auto-detected port {chosen_port}", - ) - - except Exception: - self._motor_client = None - - print("Please select a port from available devices:") - chosen_port = get_and_choose_port() - if chosen_port is None: - return False, "Connection failed: No port selected" + return False, f"Connection failed on {port}: {str(e)}" - try: - self.config = dataclasses.replace(self.config, port=chosen_port) - self._motor_client = self._create_motor_client() - with self._motor_lock: - self._motor_client.connect() - update_yaml(self.config.config_path, "port", chosen_port) - return True, f"Connection successful with port {chosen_port}" - except Exception as e2: - self._motor_client = None - return False, f"Connection failed with selected port: {str(e2)}" + def _persist_resolved_port(self, port: str) -> None: + """Write the resolved port back to config.yaml when it differs.""" + if self.config.port == port: + return + self.config = dataclasses.replace(self.config, port=port) + update_yaml(self.config.config_path, "port", port) def disconnect(self) -> tuple[bool, str]: """Disable torque and close the serial connection. @@ -1171,9 +1229,19 @@ class MockOrcaHand(OrcaHand): port is opened and motor state is simulated in memory. """ - def _create_motor_client(self) -> MotorClient: + def _resolve_port(self) -> str: + return self.config.port or "/dev/null" + + def _trial_probe(self, port: str) -> tuple[str, int]: + # Mock hand has no real bus to probe — pretend Dynamixel @ 1M. + return "dynamixel", 1_000_000 + + def _create_motor_client( + self, + motor_type: str, + port: str, + baudrate: int, + ) -> MotorClient: from .hardware.mock_dynamixel_client import MockDynamixelClient - return MockDynamixelClient( - self.config.motor_ids, self.config.port, self.config.baudrate - ) + return MockDynamixelClient(self.config.motor_ids, port, baudrate) diff --git a/orca_core/models/v1/orcahand_left/config.yaml b/orca_core/models/v1/orcahand_left/config.yaml index ceb7f7c3..17a3886f 100644 --- a/orca_core/models/v1/orcahand_left/config.yaml +++ b/orca_core/models/v1/orcahand_left/config.yaml @@ -1,6 +1,4 @@ -port: /dev/cu.usbserial-FTAAMLDU version: 0.2.1 -baudrate: 3000000 max_current: 400 type: right control_mode: current_based_position diff --git a/orca_core/models/v1/orcahand_right/config.yaml b/orca_core/models/v1/orcahand_right/config.yaml index 0a1e9c02..34b700d4 100644 --- a/orca_core/models/v1/orcahand_right/config.yaml +++ b/orca_core/models/v1/orcahand_right/config.yaml @@ -1,7 +1,4 @@ version: 0.2.1 -baudrate: 3000000 -#port: /dev/ttyUSB0 # linux -port: /dev/tty.usbserial-FT9MISJT # mac max_current: 400 type: right diff --git a/orca_core/models/v2/orcahand_left/config.yaml b/orca_core/models/v2/orcahand_left/config.yaml index 864e1cfb..d5de5645 100644 --- a/orca_core/models/v2/orcahand_left/config.yaml +++ b/orca_core/models/v2/orcahand_left/config.yaml @@ -1,6 +1,4 @@ -port: /dev/cu.usbmodem3101 version: 0.2.1 -baudrate: 1000000 max_current: 300 type: left control_mode: current_based_position diff --git a/orca_core/models/v2/orcahand_right/config.yaml b/orca_core/models/v2/orcahand_right/config.yaml index 61bf1456..009f09e8 100644 --- a/orca_core/models/v2/orcahand_right/config.yaml +++ b/orca_core/models/v2/orcahand_right/config.yaml @@ -1,6 +1,4 @@ -port: /dev/cu.usbmodem3101 version: 0.2.1 -baudrate: 1000000 max_current: 300 type: right control_mode: current_based_position diff --git a/orca_core/models/v2/orcahand_right_feetech/config.yaml b/orca_core/models/v2/orcahand_right_feetech/config.yaml deleted file mode 100644 index 546845f1..00000000 --- a/orca_core/models/v2/orcahand_right_feetech/config.yaml +++ /dev/null @@ -1,203 +0,0 @@ -port: /dev/cu.usbmodem1101 -version: 0.2.1 -baudrate: 1000000 -max_current: 300 -type: right -motor_type: feetech -control_mode: current_based_position -motor_ids: -- 1 -- 2 -- 3 -- 4 -- 5 -- 6 -- 7 -- 8 -- 9 -- 10 -- 11 -- 12 -- 13 -- 14 -- 15 -- 16 -- 17 -joint_ids: -- wrist -- thumb_cmc -- thumb_abd -- thumb_mcp -- thumb_dip -- index_abd -- index_mcp -- index_pip -- middle_abd -- middle_mcp -- middle_pip -- ring_abd -- ring_mcp -- ring_pip -- pinky_abd -- pinky_mcp -- pinky_pip -joint_to_motor_map: - thumb_cmc: 17 - thumb_abd: 14 - thumb_mcp: 15 - thumb_dip: 16 - index_abd: 4 - index_mcp: 3 - index_pip: 2 - middle_abd: 5 - middle_mcp: 10 - middle_pip: 9 - ring_abd: 6 - ring_mcp: -7 - ring_pip: 8 - pinky_abd: -13 - pinky_mcp: 12 - pinky_pip: -11 - wrist: -1 -joint_roms: - wrist: - - -65 - - 35 - thumb_cmc: - - -45 - - 33 - thumb_abd: - - -18 - - 55 - thumb_mcp: - - -25 - - 100 - thumb_dip: - - -15 - - 107 - index_abd: - - -30 - - 25 - index_mcp: - - -25 - - 100 - index_pip: - - -15 - - 107 - middle_abd: - - -27 - - 27 - middle_mcp: - - -25 - - 100 - middle_pip: - - -15 - - 107 - ring_abd: - - -27 - - 27 - ring_mcp: - - -25 - - 100 - ring_pip: - - -15 - - 107 - pinky_abd: - - -30 - - 30 - pinky_mcp: - - -25 - - 100 - pinky_pip: - - -15 - - 107 -neutral_position: - thumb_cmc: 0 - thumb_abd: 50 - thumb_mcp: 33 - thumb_dip: 18 - index_abd: -14 - index_mcp: 2 - index_pip: 6 - middle_abd: -4 - middle_mcp: 2 - middle_pip: 4 - ring_abd: 10 - ring_mcp: -2 - ring_pip: 8 - pinky_abd: 22 - pinky_mcp: -4 - pinky_pip: -2 - wrist: -20 -calibration_current: 300 -calibration_step_size: 0.15 -calibration_step_period: 0.0001 -calibration_num_stable: 10 -calibration_threshold: 0.01 -calibration_sequence: -- step: 1 - joints: - thumb_cmc: flex -- step: 2 - joints: - thumb_cmc: extend -- step: 3 - joints: - thumb_abd: flex -- step: 4 - joints: - thumb_abd: extend -- step: 5 - joints: - thumb_mcp: flex -- step: 6 - joints: - thumb_mcp: extend -- step: 7 - joints: - thumb_dip: flex -- step: 8 - joints: - thumb_dip: extend -- step: 9 - joints: - index_abd: flex - middle_abd: flex - ring_abd: flex - pinky_abd: flex -- step: 10 - joints: - index_abd: extend - middle_abd: extend - ring_abd: extend - pinky_abd: extend -- step: 11 - joints: - index_mcp: flex - middle_mcp: flex - ring_mcp: flex - pinky_mcp: flex -- step: 12 - joints: - index_mcp: extend - middle_mcp: extend - ring_mcp: extend - pinky_mcp: extend -- step: 13 - joints: - index_pip: flex - middle_pip: flex - ring_pip: flex - pinky_pip: flex -- step: 14 - joints: - index_pip: extend - middle_pip: extend - ring_pip: extend - pinky_pip: extend -- step: 15 - joints: - wrist: flex -- step: 16 - joints: - wrist: extend diff --git a/orca_core/models/v2/orcahand_touch_left/config.yaml b/orca_core/models/v2/orcahand_touch_left/config.yaml index 92e4bb22..82d6cdc8 100644 --- a/orca_core/models/v2/orcahand_touch_left/config.yaml +++ b/orca_core/models/v2/orcahand_touch_left/config.yaml @@ -1,6 +1,4 @@ -port: /dev/cu.usbmodem3101 version: 0.2.1 -baudrate: 1000000 max_current: 300 type: left control_mode: current_based_position diff --git a/orca_core/models/v2/orcahand_touch_right/config.yaml b/orca_core/models/v2/orcahand_touch_right/config.yaml index 82a7c179..a673c456 100644 --- a/orca_core/models/v2/orcahand_touch_right/config.yaml +++ b/orca_core/models/v2/orcahand_touch_right/config.yaml @@ -1,6 +1,4 @@ -port: /dev/cu.usbmodem3101 version: 0.2.1 -baudrate: 1000000 max_current: 300 type: right control_mode: current_based_position diff --git a/orca_core/utils/utils.py b/orca_core/utils/utils.py index 71e92dc8..0ebdbb98 100644 --- a/orca_core/utils/utils.py +++ b/orca_core/utils/utils.py @@ -227,6 +227,36 @@ def auto_detect_port(motor_type: str = "dynamixel") -> str: return None +def motor_type_for_port(port_device: str) -> str | None: + """Return the motor family whose USB VID matches ``port_device``, or None.""" + import serial.tools.list_ports + from ..constants import KNOWN_VIDS + + for port in serial.tools.list_ports.comports(): + if port.device != port_device: + continue + for motor_type, vids in KNOWN_VIDS.items(): + if port.vid in vids: + return motor_type + return None + return None + + +def find_single_usb_serial_port() -> str | None: + """Return the device path of the only attached USB serial adapter, or None. + + Filters out built-in / non-USB serial endpoints (Bluetooth, debug console) + by requiring a USB VID. Used as a last-resort port pick when no VID is + in :data:`~orca_core.constants.KNOWN_VIDS`. + """ + import serial.tools.list_ports + + ports = [p for p in serial.tools.list_ports.comports() if p.vid is not None] + if len(ports) == 1: + return ports[0].device + return None + + def get_and_choose_port() -> str: """Present an interactive terminal menu for USB port selection. diff --git a/scripts/stress_test.py b/scripts/stress_test.py index 7978e2c4..8fce015f 100644 --- a/scripts/stress_test.py +++ b/scripts/stress_test.py @@ -5,7 +5,7 @@ from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand -from orca_core.hardware.motor_client import MotionTimeoutError +from orca_core.constants import NUM_STEPS, STEP_SIZE MAX_TEMP = 70 # °C — conservative across Dynamixel XC330/430 and Feetech STS3215 @@ -110,20 +110,16 @@ def main() -> int: ) add_hand_arguments(parser) parser.add_argument( - "--num-steps", type=int, default=25, - help="Interpolation steps per move (default 25)." + "--num-steps", type=int, default=NUM_STEPS, + help=f"Interpolation steps per move (default {NUM_STEPS})." ) parser.add_argument( - "--step-size", type=float, default=0.001, - help="Sleep between interpolation steps in seconds (default 0.001)." + "--step-size", type=float, default=STEP_SIZE, + help=f"Sleep between interpolation steps in seconds (default {STEP_SIZE})." ) parser.add_argument( - "--hold", type=float, default=0.0, - help="Extra seconds to hold each pose AFTER motion completes (default 0)." - ) - parser.add_argument( - "--motion-timeout", type=float, default=5.0, - help="Max seconds to wait for a pose to be reached (default 5)." + "--hold", type=float, default=2.0, + help="Seconds to hold each pose AFTER motion completes (default 2)." ) args = parser.parse_args() @@ -147,20 +143,12 @@ def main() -> int: hand.set_joint_positions( JOINT_OPEN, num_steps=args.num_steps, step_size=args.step_size ) - try: - hand.wait_for_motion(timeout=args.motion_timeout) - except MotionTimeoutError as exc: - print(f"{YELLOW}Motion timed out: {exc}{RST}") if args.hold: time.sleep(args.hold) hand.set_joint_positions( JOINT_CLOSE, num_steps=args.num_steps, step_size=args.step_size ) - try: - hand.wait_for_motion(timeout=args.motion_timeout) - except MotionTimeoutError as exc: - print(f"{YELLOW}Motion timed out: {exc}{RST}") if args.hold: time.sleep(args.hold) except KeyboardInterrupt: diff --git a/tests/test_connect_resolution.py b/tests/test_connect_resolution.py new file mode 100644 index 00000000..95d3c238 --- /dev/null +++ b/tests/test_connect_resolution.py @@ -0,0 +1,144 @@ +"""Tests for OrcaHand.connect() driver auto-detection.""" + +import dataclasses +from types import SimpleNamespace + +import pytest + +from orca_core.constants import KNOWN_VIDS +from orca_core.hardware_hand import MockOrcaHand +from orca_core.utils.utils import ( + find_single_usb_serial_port, + motor_type_for_port, +) + + +DYNAMIXEL_VID = KNOWN_VIDS["dynamixel"][0] +FEETECH_VID = KNOWN_VIDS["feetech"][0] + + +def _fake_port(device: str, vid: int) -> SimpleNamespace: + return SimpleNamespace(device=device, vid=vid, description="fake") + + +@pytest.fixture +def patch_comports(monkeypatch): + def _set(ports): + import serial.tools.list_ports as ltp + monkeypatch.setattr(ltp, "comports", lambda: ports) + return _set + + +# ----- USB VID lookup helpers --------------------------------------------- + +def test_motor_type_for_port_matches_known_vid(patch_comports): + patch_comports([_fake_port("/dev/cu.feetech", FEETECH_VID)]) + assert motor_type_for_port("/dev/cu.feetech") == "feetech" + + +def test_motor_type_for_port_returns_none_for_unknown_vid(patch_comports): + patch_comports([_fake_port("/dev/cu.weird", 0xDEAD)]) + assert motor_type_for_port("/dev/cu.weird") is None + + +def test_find_single_usb_returns_lone_adapter(patch_comports): + patch_comports([_fake_port("/dev/cu.weird", 0x2F5D)]) + assert find_single_usb_serial_port() == "/dev/cu.weird" + + +def test_find_single_usb_returns_none_when_multiple(patch_comports): + patch_comports( + [ + _fake_port("/dev/cu.a", 0x2F5D), + _fake_port("/dev/cu.b", 0x2F5D), + ] + ) + assert find_single_usb_serial_port() is None + + +def test_find_single_usb_skips_non_usb_ports(patch_comports): + patch_comports( + [ + SimpleNamespace(device="/dev/cu.bluetooth", vid=None, description=""), + _fake_port("/dev/cu.usb", 0x2F5D), + ] + ) + assert find_single_usb_serial_port() == "/dev/cu.usb" + + +# ----- _trial_probe ------------------------------------------------------- + +@pytest.fixture +def mock_hand(mock_config_dir): + return MockOrcaHand(config_path=str(mock_config_dir / "config.yaml")) + + +def test_trial_probe_finds_feetech(mock_hand, monkeypatch): + from orca_core.hardware import dynamixel_client, feetech_client + monkeypatch.setattr( + dynamixel_client.DynamixelClient, "probe", staticmethod(lambda *a, **k: False) + ) + monkeypatch.setattr( + feetech_client.FeetechClient, + "probe", + staticmethod(lambda port, baudrate, motor_ids, **k: True), + ) + motor_type, baudrate = OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + assert motor_type == "feetech" + assert baudrate == 1_000_000 + + +def test_trial_probe_finds_dynamixel_at_3M(mock_hand, monkeypatch): + """v1 hands run Dynamixels at 3M; probe must iterate past the 1M default.""" + seen = [] + def fake_probe(port, baudrate, motor_ids, **k): + seen.append(baudrate) + return baudrate == 3_000_000 + from orca_core.hardware import dynamixel_client, feetech_client + monkeypatch.setattr( + dynamixel_client.DynamixelClient, "probe", staticmethod(fake_probe) + ) + monkeypatch.setattr( + feetech_client.FeetechClient, "probe", staticmethod(lambda *a, **k: False) + ) + motor_type, baudrate = OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + assert motor_type == "dynamixel" + assert baudrate == 3_000_000 + assert seen == [1_000_000, 3_000_000] # priority order + + +def test_trial_probe_returns_none_when_nothing_responds(mock_hand, monkeypatch): + from orca_core.hardware import dynamixel_client, feetech_client + monkeypatch.setattr( + dynamixel_client.DynamixelClient, "probe", staticmethod(lambda *a, **k: False) + ) + monkeypatch.setattr( + feetech_client.FeetechClient, "probe", staticmethod(lambda *a, **k: False) + ) + assert OrcaHand_trial_probe(mock_hand, "/dev/cu.x") == (None, None) + + +def test_trial_probe_honours_pinned_motor_type(mock_hand, monkeypatch): + """When motor_type is pinned in yaml, only baudrates iterate.""" + mock_hand.config = dataclasses.replace(mock_hand.config, motor_type="dynamixel") + seen_types = set() + def fake_probe(port, baudrate, motor_ids, **k): + return False # never responds — we just want to see the iteration + from orca_core.hardware import dynamixel_client, feetech_client + def feetech_probe(*a, **k): + seen_types.add("feetech") + return False + monkeypatch.setattr( + dynamixel_client.DynamixelClient, "probe", staticmethod(fake_probe) + ) + monkeypatch.setattr( + feetech_client.FeetechClient, "probe", staticmethod(feetech_probe) + ) + OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + assert "feetech" not in seen_types + + +def OrcaHand_trial_probe(hand, port): + """Hop over the mock-class override to test the real implementation.""" + from orca_core.hardware_hand import OrcaHand + return OrcaHand._trial_probe(hand, port) From 04740f36d92ac50c00c744e4a601b9638b23b1e4 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Wed, 6 May 2026 15:56:58 +0200 Subject: [PATCH 03/10] Add Feetech support to configure_motor_chain + per-joint calibration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Brings the configure_motor_chain script and core calibration logic to parity with the Feetech build of the hand. Ported from fb/dev with adaptations for the auto-detect machinery introduced earlier on this branch. configure_motor_chain.py - Optional --motor-type with auto-detection at factory defaults (Dynamixel @ 57600 ID 1, Feetech @ 1M ID 1) when omitted - --reset to revert configured motors back to factory defaults - --baudrate to bulk-change baud without touching IDs - Safe power-cycle prompt for Feetech (unkeyed connectors → reverse polarity destroys the motor; force unplug-replug for each insert) - HLS3930 wrist + HLS3915 fingers split with substring model checks, matching the existing XC430/XC330 split for Dynamixel - _close_silently() helper used after each ID/baud mutation so the spurious "no status packet" disconnect logs go away - Pre-scan filter recognises HLS3930 at ID 1 as a configured wrist (not a factory-default to ignore) - Positional arg renamed model_path -> config_path for parity with every other script in the repo FeetechClient - scan_for_motors / change_motor_id / change_motor_baudrate - FEETECH_BAUD_RATE_MAP (host-baud -> register code) - FEETECH_MODELS populated for the three model_number values seen in our 17-motor builds: 4106 -> HLS3930, 6922/5130 -> HLS3915 Calibration core - OrcaHand.calibrate(joints=...) and _calibrate(joints=...) filter the calibration sequence by an explicit joint allow-list. Lets you re-run one finger or one joint without touching the others. The scripts/calibrate.py CLI for this is intentionally a follow-up commit. Bundled configs - Each model's config.yaml gets a commented-out `port:` / `baudrate:` hint at the top so users can find the override path quickly without hunting through docs utils - Re-export auto_detect_port and get_and_choose_port from the package root so scripts can `from orca_core.utils import ...` directly --- orca_core/hardware/feetech_client.py | 119 ++++ orca_core/hardware_hand.py | 39 +- orca_core/models/v1/orcahand_left/config.yaml | 2 + .../models/v1/orcahand_right/config.yaml | 2 + orca_core/models/v2/orcahand_left/config.yaml | 2 + .../models/v2/orcahand_right/config.yaml | 1 + .../models/v2/orcahand_touch_left/config.yaml | 2 + .../v2/orcahand_touch_right/config.yaml | 2 + orca_core/utils/__init__.py | 11 +- scripts/configure_motor_chain.py | 580 +++++++++++++++--- 10 files changed, 657 insertions(+), 103 deletions(-) diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index c9f87e71..ba9ade10 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -32,8 +32,32 @@ SMS_STS_GOAL_POSITION_L, SMS_STS_GOAL_TIME_L, SMS_STS_GOAL_SPEED_L, + SMS_STS_ID, + SMS_STS_BAUD_RATE, ) +# Map host-facing baud rates to the firmware's register code. +FEETECH_BAUD_RATE_MAP: dict[int, int] = { + 1_000_000: 0, + 500_000: 1, + 250_000: 2, + 128_000: 3, + 115_200: 4, + 76_800: 5, + 57_600: 6, + 38_400: 7, +} + +# Model-number → human name. The string is used by configure_motor_chain +# for substring-matching, so multiple raw model numbers can share a label +# (e.g., HLS3915 ships in two firmware revisions reporting different +# model_number values; both are functionally HLS3915). +FEETECH_MODELS: dict[int, str] = { + 4106: 'HLS3930', + 6922: 'HLS3915', + 5130: 'HLS3915', +} + # SCServo position scale: 0-4095 raw units = 0-360 degrees = 0-2*pi radians DEFAULT_POS_SCALE = 2.0 * np.pi / 4096 # 4096 steps for 360° DEFAULT_VEL_SCALE = 0.732 * 2.0 * np.pi / 60.0 # Convert 0.732 RPM/unit to rad/s @@ -173,6 +197,101 @@ def disconnect(self) -> None: if self in self.OPEN_CLIENTS: self.OPEN_CLIENTS.remove(self) + @staticmethod + def scan_for_motors( + port: str = '/dev/ttyUSB0', + id_range: tuple = (0, 252), + baud_rates: list = None, + ) -> list: + """Scan ``port`` for any responding Feetech motors. + + Tries each baud in ``baud_rates`` (defaults to all known rates), pings + every ID in ``id_range``, and returns a list of dicts with keys + ``id``, ``baud_rate``, ``model_name``. Doesn't change motor state. + """ + baud_rates = baud_rates or list(FEETECH_BAUD_RATE_MAP.keys()) + detected_motors = [] + for baud_rate in baud_rates: + port_handler = PortHandler(port) + port_handler.baudrate = baud_rate + try: + if not port_handler.openPort(): + logging.warning( + 'Failed to open port %s at %d baud', port, baud_rate + ) + continue + packet_handler = sms_sts(port_handler) + for motor_id in range(id_range[0], id_range[1] + 1): + model_number, result, _ = packet_handler.ping(motor_id) + if result == COMM_SUCCESS: + detected_motors.append({ + 'id': motor_id, + 'baud_rate': baud_rate, + 'model_name': FEETECH_MODELS.get(model_number, 'Feetech'), + }) + port_handler.closePort() + except Exception as e: + logging.warning( + 'Error scanning port %s at %d baud: %s', port, baud_rate, e + ) + try: + port_handler.closePort() + except Exception: + pass + return detected_motors + + def change_motor_id(self, current_id: int, new_id: int) -> bool: + """Re-program a motor's bus ID. Persists in EEPROM.""" + if not (0 <= new_id <= 253): + logging.error("Invalid ID %d. Valid range is 0-253.", new_id) + return False + try: + self._check_connected() + self.set_torque_enabled([current_id], False, retries=0) + self.packet_handler.unLockEprom(current_id) + result, error = self.packet_handler.write1ByteTxRx( + current_id, SMS_STS_ID, new_id + ) + self.packet_handler.LockEprom(new_id) + if result == COMM_SUCCESS and error == 0: + logging.info("Changed motor ID: %d -> %d", current_id, new_id) + return True + logging.error( + "Failed to change motor ID: result=%d, error=%d", result, error + ) + return False + except Exception as e: + logging.error("Failed to change motor ID: %s", e) + return False + + def change_motor_baudrate(self, motor_id: int, new_baud_rate: int) -> bool: + """Re-program a motor's UART baud. Caller must reconnect at the new rate.""" + if new_baud_rate not in FEETECH_BAUD_RATE_MAP: + logging.error( + "Invalid baud rate %d. Valid: %s", + new_baud_rate, + list(FEETECH_BAUD_RATE_MAP.keys()), + ) + return False + try: + self._check_connected() + self.set_torque_enabled([motor_id], False, retries=0) + self.packet_handler.unLockEprom(motor_id) + result, error = self.packet_handler.write1ByteTxRx( + motor_id, SMS_STS_BAUD_RATE, FEETECH_BAUD_RATE_MAP[new_baud_rate] + ) + self.packet_handler.LockEprom(motor_id) + if result == COMM_SUCCESS and error == 0: + logging.info("Changed motor %d baud rate to %d", motor_id, new_baud_rate) + return True + logging.error( + "Failed to change baud rate: result=%d, error=%d", result, error + ) + return False + except Exception as e: + logging.error("Failed to change baud rate: %s", e) + return False + def set_torque_enabled( self, motor_ids: Sequence[int], diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index e59b6b93..af62c4f4 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -548,7 +548,12 @@ def is_calibrated(self, verbose: bool = False) -> bool: return overall_calibrated - def calibrate(self, blocking: bool = True, force_wrist: bool = False): + def calibrate( + self, + blocking: bool = True, + force_wrist: bool = False, + joints: list | None = None, + ): """Run the joint calibration routine. Drives each joint to its mechanical limits in the sequence defined by @@ -556,17 +561,24 @@ def calibrate(self, blocking: bool = True, force_wrist: bool = False): each limit, and persists the resulting motor limits and joint-to-motor ratios to ``calibration.yaml``. + Args: + blocking: When True, runs to completion before returning. + force_wrist: Recalibrate the wrist even if it's already calibrated. + joints: When given, only calibrate steps that touch any of these + joint names (others are skipped). Useful for re-running just + one finger or joint without re-calibrating the whole hand. + On completion ``self.calibration`` is replaced with a fresh :class:`~orca_core.CalibrationResult`. Partial progress is written to disk after every step so an interrupted run is never fully lost. """ if blocking: self._task_stop_event.clear() - result = self._calibrate(force_wrist=force_wrist) + result = self._calibrate(force_wrist=force_wrist, joints=joints) if result is not None: self.calibration = result else: - self._start_task(self._calibrate_and_apply, force_wrist=force_wrist) + self._start_task(self._calibrate_and_apply, force_wrist=force_wrist, joints=joints) def _build_calibration_result( self, @@ -591,7 +603,11 @@ def _build_calibration_result( wrist_calibrated=wrist_calibrated, ) - def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: + def _calibrate( + self, + force_wrist: bool = False, + joints: list | None = None, + ) -> CalibrationResult | None: """Execute the calibration routine and return a :class:`~orca_core.CalibrationResult`. Drives each joint through its mechanical limits following ``calibration_sequence`` @@ -628,6 +644,21 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: {STEP: len(calibration_sequence) + 1, JOINTS: {WRIST: EXTEND}} ) + if joints is not None: + joints_set = set(joints) + filtered = [] + for step in calibration_sequence: + step_joints = { + j: d for j, d in step[JOINTS].items() if j in joints_set + } + if step_joints: + filtered.append({STEP: step[STEP], JOINTS: step_joints}) + print( + f"Calibrating {len(joints_set)} joint(s) across {len(filtered)} " + f"step(s) (out of {len(calibration_sequence)} total)." + ) + calibration_sequence = filtered + # Deep-copy current limits so per-step YAML writes reflect only the # joints being calibrated, not stale data from a prior incomplete run. motor_limits = { diff --git a/orca_core/models/v1/orcahand_left/config.yaml b/orca_core/models/v1/orcahand_left/config.yaml index 17a3886f..77cb9fcc 100644 --- a/orca_core/models/v1/orcahand_left/config.yaml +++ b/orca_core/models/v1/orcahand_left/config.yaml @@ -1,3 +1,5 @@ +# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) +# baudrate: 3000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 400 type: right diff --git a/orca_core/models/v1/orcahand_right/config.yaml b/orca_core/models/v1/orcahand_right/config.yaml index 34b700d4..ef07d27d 100644 --- a/orca_core/models/v1/orcahand_right/config.yaml +++ b/orca_core/models/v1/orcahand_right/config.yaml @@ -1,3 +1,5 @@ +# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) +# baudrate: 3000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 400 diff --git a/orca_core/models/v2/orcahand_left/config.yaml b/orca_core/models/v2/orcahand_left/config.yaml index d5de5645..40eff86b 100644 --- a/orca_core/models/v2/orcahand_left/config.yaml +++ b/orca_core/models/v2/orcahand_left/config.yaml @@ -1,3 +1,5 @@ +# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) +# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: left diff --git a/orca_core/models/v2/orcahand_right/config.yaml b/orca_core/models/v2/orcahand_right/config.yaml index 009f09e8..e2be1657 100644 --- a/orca_core/models/v2/orcahand_right/config.yaml +++ b/orca_core/models/v2/orcahand_right/config.yaml @@ -1,3 +1,4 @@ +port: /dev/cu.usbmodem1101 version: 0.2.1 max_current: 300 type: right diff --git a/orca_core/models/v2/orcahand_touch_left/config.yaml b/orca_core/models/v2/orcahand_touch_left/config.yaml index 82d6cdc8..10c3cc8c 100644 --- a/orca_core/models/v2/orcahand_touch_left/config.yaml +++ b/orca_core/models/v2/orcahand_touch_left/config.yaml @@ -1,3 +1,5 @@ +# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) +# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: left diff --git a/orca_core/models/v2/orcahand_touch_right/config.yaml b/orca_core/models/v2/orcahand_touch_right/config.yaml index a673c456..64a45d0c 100644 --- a/orca_core/models/v2/orcahand_touch_right/config.yaml +++ b/orca_core/models/v2/orcahand_touch_right/config.yaml @@ -1,3 +1,5 @@ +# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) +# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: right diff --git a/orca_core/utils/__init__.py b/orca_core/utils/__init__.py index 1fc694f7..4a2ad3aa 100644 --- a/orca_core/utils/__init__.py +++ b/orca_core/utils/__init__.py @@ -1 +1,10 @@ -from .utils import read_yaml, update_yaml, get_model_path, linear_interp, ease_in_out, interpolate_waypoints +from .utils import ( + read_yaml, + update_yaml, + get_model_path, + linear_interp, + ease_in_out, + interpolate_waypoints, + auto_detect_port, + get_and_choose_port, +) diff --git a/scripts/configure_motor_chain.py b/scripts/configure_motor_chain.py index 79794625..1c45f28f 100755 --- a/scripts/configure_motor_chain.py +++ b/scripts/configure_motor_chain.py @@ -6,10 +6,10 @@ import subprocess sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from orca_core.hardware.dynamixel_client import DynamixelClient -from orca_core.utils import read_yaml, get_model_path +from orca_core.hardware.dynamixel_client import DynamixelClient, BAUD_RATE_MAP as DYNAMIXEL_BAUD_RATE_MAP +from orca_core.hardware.feetech_client import FeetechClient, FEETECH_BAUD_RATE_MAP +from orca_core.utils import read_yaml, get_model_path, auto_detect_port, get_and_choose_port -DEFAULT_ID, DEFAULT_BAUD = 1, 57600 RED = '\033[91m' GREEN = '\033[92m' BLUE = '\033[94m' @@ -20,8 +20,124 @@ UNDERLINE = '\033[4m' RESET = '\033[0m' +def validate_or_detect_port(port: str, config_path: str, motor_type: str) -> str: + """Check if port exists, auto-detect or prompt if not.""" + if os.path.exists(port): + return port + + print(f"{YELLOW}⚠ Port {port} not found.{RESET}") + + detected = auto_detect_port(motor_type) + if detected and os.path.exists(detected): + print(f"{GREEN}✓ Using auto-detected port: {detected}{RESET}") + return detected + + print("Please select a port from available devices:") + chosen = get_and_choose_port() + if chosen: + print(f"{GREEN}✓ Using selected port: {chosen}{RESET}") + return chosen + + print(f"{RED}❌ No valid port found. Check your USB connection.{RESET}") + sys.exit(1) + + +def wait_for_port_removed(port: str, timeout: float = 30): + """Wait until the serial port disappears (USB unplugged).""" + print(f"\n{YELLOW}⚠ REMOVE POWER:{RESET} Unplug the USB cable from your computer.") + print(f" Waiting for {BOLD}{port}{RESET} to disappear...") + start = time.time() + while os.path.exists(port): + if time.time() - start > timeout: + print(f"{RED}❌ Timeout: port {port} still exists. Unplug the USB cable.{RESET}") + start = time.time() + time.sleep(0.3) + print(f"{GREEN}✓ USB disconnected.{RESET}") + + +def wait_for_port_reconnected(port: str, timeout: float = 60): + """Wait until the serial port reappears (USB plugged back in).""" + print(f"\n{YELLOW}▶ RESTORE POWER:{RESET} Plug the USB cable back in.") + print(f" Waiting for {BOLD}{port}{RESET} to reappear...") + start = time.time() + while not os.path.exists(port): + if time.time() - start > timeout: + print(f"{RED}❌ Timeout: port {port} not found. Plug in the USB cable.{RESET}") + start = time.time() + time.sleep(0.3) + time.sleep(0.5) + print(f"{GREEN}✓ USB reconnected on {port}.{RESET}") + + +def feetech_safe_connect_prompt(port: str, connection_msg: str): + """Guide user through safe power-off motor connection for Feetech servos.""" + wait_for_port_removed(port) + print(f"\n{ORANGE}🔌 {connection_msg}{RESET}") + input(f"\n Press {BOLD}Enter{RESET} when the motor is connected correctly...") + wait_for_port_reconnected(port) + + +MOTOR_TYPE_DEFAULTS = { + 'dynamixel': {'default_id': 1, 'default_baud': 57600}, + 'feetech': {'default_id': 1, 'default_baud': 1000000}, +} + +def create_config_client(motor_type, motor_ids, port, baudrate): + if motor_type == 'feetech': + return FeetechClient(motor_ids, port=port, baudrate=baudrate) + return DynamixelClient(motor_ids, port=port, baudrate=baudrate) + + +def _close_silently(client) -> None: + """Close the port without sending torque-disable. + + During motor (re)configuration the motor's bus ID or baudrate changes + mid-session, so a normal ``client.disconnect()`` would fire a torque- + disable packet at an address the motor no longer answers on, producing + spurious "no status packet" errors. Closing the port directly avoids + the failed write while still releasing the OS handle. + """ + try: + client.port_handler.closePort() + except Exception: + pass + + +def detect_motor_type(port: str) -> str | None: + """Probe ``port`` at each family's factory defaults to identify motors. + + Fresh motors ship at known (baud, ID) pairs that differ between families: + Dynamixel @ 57600 ID 1, Feetech @ 1M ID 1. Pinging at these defaults + tells us which protocol the bus speaks without needing user input. + + Returns ``"dynamixel"``, ``"feetech"``, or ``None`` when nothing responds + (no motor connected, motor not at factory defaults, or wiring issue). + """ + for motor_type, defaults in MOTOR_TYPE_DEFAULTS.items(): + baud = defaults['default_baud'] + default_id = defaults['default_id'] + print( + f" Probing {motor_type} at factory defaults " + f"(ID {default_id} @ {baud} baud)...", + end=' ', + flush=True, + ) + try: + client = create_config_client(motor_type, [], port, baud) + motors = client.scan_for_motors( + port=port, + id_range=(default_id, default_id), + baud_rates=[baud], + ) + if motors: + print(f"{GREEN}found.{RESET}") + return motor_type + print("nothing.") + except Exception as e: + print(f"{YELLOW}error ({e}).{RESET}") + return None + def play_success_beep(): - """Play a short beep to indicate successful motor configuration.""" try: subprocess.run(['paplay', '/usr/share/sounds/freedesktop/stereo/complete.oga'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) except: @@ -30,15 +146,24 @@ def play_success_beep(): except: print('\a', end='', flush=True) -def scan_for_default_motor(expected_type: str, port: str) -> bool: +def motor_color(model_name): + # Finger motors → blue; wrist motors → purple. Both motor families + # share the same role split, so the colour scheme matches. + if 'XC330' in model_name or 'HLS3915' in model_name: + return BLUE + if 'XC430' in model_name or 'HLS3930' in model_name: + return PURPLE + return '' + +def scan_for_default_motor(expected_type: str, port: str, motor_type: str, default_id: int, default_baud: int) -> bool: try: - client = DynamixelClient([], port=port, baudrate=DEFAULT_BAUD) - motors = client.scan_for_motors(port=port, id_range=(DEFAULT_ID, DEFAULT_ID), baud_rates=[DEFAULT_BAUD]) + client = create_config_client(motor_type, [], port, default_baud) + motors = client.scan_for_motors(port=port, id_range=(default_id, default_id), baud_rates=[default_baud]) if not motors: return False - motor=motors[0] - expected_color = BLUE if 'XC330' in expected_type else PURPLE if 'XC430' in expected_type else '' - actual_color = BLUE if 'XC330' in motor['model_name'] else PURPLE if 'XC430' in motor['model_name'] else '' + motor = motors[0] + expected_color = motor_color(expected_type) + actual_color = motor_color(motor['model_name']) if expected_type in motor['model_name']: print(f"{GREEN}✓ Found default {actual_color}{motor['model_name']}{GREEN} motor{RESET}") return True @@ -49,24 +174,26 @@ def scan_for_default_motor(expected_type: str, port: str) -> bool: print(f"{RED}❌ Scan error: {e}{RESET}") return False -def configure_default_motor(target_id: int, port: str, target_baud: int) -> bool: - """Configure motor with default settings (ID=1, baudrate=57600) to Target ID and Target Baudrate.""" +def configure_default_motor(target_id: int, port: str, target_baud: int, motor_type: str, default_id: int, default_baud: int) -> bool: try: - client = DynamixelClient([DEFAULT_ID], port=port, baudrate=DEFAULT_BAUD) + client = create_config_client(motor_type, [default_id], port, default_baud) client.connect() - if not client.change_motor_id(DEFAULT_ID, target_id): + if not client.change_motor_id(default_id, target_id): print(f"{RED}❌ Failed to change ID to {target_id}{RESET}") - client.port_handler.closePort() + _close_silently(client) return False - client.port_handler.closePort() + _close_silently(client) time.sleep(0.5) - client = DynamixelClient([target_id], port=port, baudrate=DEFAULT_BAUD) - client.connect() - if not client.change_motor_baudrate(target_id, target_baud): - print(f"{RED}❌ Failed to change baud rate to {target_baud}{RESET}") - client.port_handler.closePort() - return False - client.port_handler.closePort() + + if target_baud != default_baud: + client = create_config_client(motor_type, [target_id], port, default_baud) + client.connect() + if not client.change_motor_baudrate(target_id, target_baud): + print(f"{RED}❌ Failed to change baud rate to {target_baud}{RESET}") + _close_silently(client) + return False + _close_silently(client) + print(f"{GREEN}✓ Successfully configured the new motor to ID={target_id}, baudrate={target_baud}{RESET}") play_success_beep() return True @@ -74,66 +201,95 @@ def configure_default_motor(target_id: int, port: str, target_baud: int) -> bool print(f"{RED}❌ Error configuring motor: {e}{RESET}") return False -def scan_already_configured_motors(port: str, target_baud: int, total_motors: int, xc430_id: int = None) -> list: - """Scan for pre-configured motors and validate sequence.""" +def scan_already_configured_motors(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int, xc430_id: int = None) -> list: print(f"\n🔍 Pre-Scanning for {YELLOW}already configured{RESET} motors...") try: - client = DynamixelClient([], port=port, baudrate=target_baud) + client = create_config_client(motor_type, [], port, target_baud) motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) + # When target_baud == default_baud (Feetech), a factory-default motor + # sits at default_id with the finger model (HLS3915), so we drop it + # from the pre-scan to avoid mistaking it for a configured wrist. + # A genuine wrist (HLS3930 at ID 1) is kept — the model name disambiguates. + if target_baud == default_baud: + motors = [ + m for m in motors + if m['id'] != default_id or 'HLS3930' in m['model_name'] + ] if not motors: print(" No pre-configured motors detected") return [] motor_by_id = {m['id']: m for m in motors} valid_sequence, expected_id = [], total_motors - min_xc330_id = 2 if xc430_id is not None else 1 - - for motor_id in sorted(motor_by_id.keys(), reverse=True): - if motor_id in range(min_xc330_id, total_motors + 1) and motor_id == expected_id: - if 'XC330' in motor_by_id[motor_id]['model_name']: - valid_sequence.append(motor_id) - expected_id -= 1 - else: + + if motor_type == 'feetech': + min_finger_id = 2 if xc430_id is not None else 1 + for mid in sorted(motor_by_id.keys(), reverse=True): + if mid in range(min_finger_id, total_motors + 1) and mid == expected_id: + if 'HLS3915' in motor_by_id[mid]['model_name']: + valid_sequence.append(mid) + expected_id -= 1 + else: + break + elif mid not in range(min_finger_id, total_motors + 1): break - elif motor_id not in range(min_xc330_id, total_motors + 1): - break - if xc430_id is not None and xc430_id in motor_by_id and expected_id == 1: - if 'XC430' in motor_by_id[xc430_id]['model_name']: - valid_sequence.append(xc430_id) - invalid_motors = [id for id in motor_by_id.keys() if id not in valid_sequence] - - # Output results + if xc430_id is not None and xc430_id in motor_by_id and expected_id == 1: + if 'HLS3930' in motor_by_id[xc430_id]['model_name']: + valid_sequence.append(xc430_id) + else: + min_xc330_id = 2 if xc430_id is not None else 1 + for mid in sorted(motor_by_id.keys(), reverse=True): + if mid in range(min_xc330_id, total_motors + 1) and mid == expected_id: + if 'XC330' in motor_by_id[mid]['model_name']: + valid_sequence.append(mid) + expected_id -= 1 + else: + break + elif mid not in range(min_xc330_id, total_motors + 1): + break + if xc430_id is not None and xc430_id in motor_by_id and expected_id == 1: + if 'XC430' in motor_by_id[xc430_id]['model_name']: + valid_sequence.append(xc430_id) + + invalid_motors = [mid for mid in motor_by_id.keys() if mid not in valid_sequence] + if valid_sequence: print(f"{GREEN}\nFound {len(valid_sequence)} valid, pre-configured motors with correct ID order:{RESET}") - for motor_id in sorted(valid_sequence, reverse=True): - motor = motor_by_id[motor_id] - color = BLUE if 'XC330' in motor['model_name'] else PURPLE if 'XC430' in motor['model_name'] else '' - print(f" • ID {motor_id:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") + for mid in sorted(valid_sequence, reverse=True): + motor = motor_by_id[mid] + color = motor_color(motor['model_name']) + print(f" • ID {mid:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") if invalid_motors: print(f"{RED}Found {len(invalid_motors)} motors with invalid configuration:{RESET}") - for motor_id in sorted(invalid_motors, reverse=True): - motor = motor_by_id[motor_id] - color = BLUE if 'XC330' in motor['model_name'] else PURPLE if 'XC430' in motor['model_name'] else '' - print(f" • ID {motor_id:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") + for mid in sorted(invalid_motors, reverse=True): + motor = motor_by_id[mid] + color = motor_color(motor['model_name']) + print(f" • ID {mid:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") print(f"{YELLOW}\nExpected sequence for {len(motor_by_id)} connected motors: {RESET}") for i in range(total_motors, total_motors - len(motor_by_id), -1): if xc430_id is not None and i == 1: - print(f" • ID 1: {PURPLE}XC430-T240BB-T{RESET} @ {target_baud:,} bps") + if motor_type == 'feetech': + print(f" • ID 1: {PURPLE}HLS3930{RESET} @ {target_baud:,} bps") + else: + print(f" • ID 1: {PURPLE}XC430-T240BB-T{RESET} @ {target_baud:,} bps") else: - print(f" • ID {i:2d}: {BLUE}XC330-T288-T{RESET} @ {target_baud:,} bps") + if motor_type == 'feetech': + print(f" • ID {i:2d}: {BLUE}HLS3915{RESET} @ {target_baud:,} bps") + else: + print(f" • ID {i:2d}: {BLUE}XC330-T288-T{RESET} @ {target_baud:,} bps") + defaults = MOTOR_TYPE_DEFAULTS[motor_type] print(f"{RED}\n🚫 CONFIGURATION CANNOT CONTINUE{RESET}") - print(f" {RED}Please reset the relevant motors to factory default (ID=1, baud=57,600) and restart.{RESET}") + print(f" {RED}Please reset the relevant motors to factory default (ID={defaults['default_id']}, baud={defaults['default_baud']:,}) and restart.{RESET}") sys.exit(1) return valid_sequence except Exception as e: print(f"{RED}❌ Error scanning: {e}{RESET}") return [] -def verify_all_motors(configured_ids: list, port: str, target_baud: int, total_motors: int) -> bool: - """Verify that all previously configured motors are still detected.""" +def verify_all_motors(configured_ids: list, port: str, target_baud: int, total_motors: int, motor_type: str) -> bool: if not configured_ids: - return True + return True try: - client = DynamixelClient([], port=port, baudrate=target_baud) + client = create_config_client(motor_type, [], port, target_baud) motors = client.scan_for_motors( port=port, id_range=(1, total_motors), @@ -148,57 +304,281 @@ def verify_all_motors(configured_ids: list, port: str, target_baud: int, total_m print(f" Expected: {expected_ids}") print(f" Found: {found_ids}") print("\n🔍 Possible causes:") - print(" • Multiple motors with same ID=1 were connected (causes conflicts)") - print(" • Motor lost power or connection during configuration)") - print("Check wiring and/or use Dynamixel Wizard to inspect motor settings and connections.") - return False + print(" • Multiple motors with same default ID were connected (causes conflicts)") + print(" • Motor lost power or connection during configuration") + if motor_type == 'dynamixel': + print("Check wiring and/or use Dynamixel Wizard to inspect motor settings and connections.") + else: + print("Check wiring and inspect motor settings.") + return False except Exception as e: print(f"{RED}❌ Error verifying motors: {e}{RESET}") return False +def get_valid_baudrates(motor_type: str) -> list: + """Get list of valid baudrates for the given motor type.""" + if motor_type == 'feetech': + return sorted(FEETECH_BAUD_RATE_MAP.keys(), reverse=True) + return sorted(DYNAMIXEL_BAUD_RATE_MAP.keys(), reverse=True) + +def change_motor_baudrate_only(motor_type: str, motor_id: int, port: str, current_baud: int, new_baud: int) -> bool: + """Change only the baudrate of a motor, leaving ID unchanged.""" + try: + if current_baud == new_baud: + print(f"{YELLOW} Motor {motor_id} already at {new_baud:,} bps{RESET}") + return True + + client = create_config_client(motor_type, [motor_id], port, current_baud) + client.connect() + if not client.change_motor_baudrate(motor_id, new_baud): + print(f"{RED}❌ Failed to change baud rate for motor {motor_id}{RESET}") + _close_silently(client) + return False + _close_silently(client) + + print(f"{GREEN}✓ Changed motor {motor_id} baudrate: {current_baud:,} → {new_baud:,}{RESET}") + return True + except Exception as e: + print(f"{RED}❌ Error changing baudrate for motor {motor_id}: {e}{RESET}") + return False + +def reset_motor_to_factory(motor_type: str, motor_id: int, port: str, current_baud: int, default_id: int, default_baud: int) -> bool: + try: + if current_baud != default_baud: + client = create_config_client(motor_type, [motor_id], port, current_baud) + client.connect() + if not client.change_motor_baudrate(motor_id, default_baud): + print(f"{RED}❌ Failed to change baud rate for motor {motor_id}{RESET}") + _close_silently(client) + return False + _close_silently(client) + time.sleep(0.5) + + client = create_config_client(motor_type, [motor_id], port, default_baud) + client.connect() + if not client.change_motor_id(motor_id, default_id): + print(f"{RED}❌ Failed to change ID for motor {motor_id}{RESET}") + _close_silently(client) + return False + _close_silently(client) + + print(f"{GREEN}✓ Reset motor {motor_id} → ID={default_id}, baudrate={default_baud:,}{RESET}") + return True + except Exception as e: + print(f"{RED}❌ Error resetting motor {motor_id}: {e}{RESET}") + return False + +def baudrate_change_loop(port: str, current_baud: int, new_baud: int, total_motors: int, motor_type: str): + """Scan for motors and change their baudrate without modifying IDs.""" + print(f"\n{BOLD}🔄 BAUDRATE CHANGE MODE{RESET} — Changing all motors to {new_baud:,} bps...") + print(f" Press {BOLD}Ctrl+C{RESET} to stop.") + + if motor_type == 'feetech': + while True: + feetech_safe_connect_prompt(port, "Connect the motor(s) you want to change baudrate") + print(f"\n🔍 Scanning for motors at {current_baud:,} bps...") + client = create_config_client(motor_type, [], port, current_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[current_baud]) + if not motors: + print(f"{YELLOW} No motors found at {current_baud:,} bps. Check connections.{RESET}") + continue + for motor in motors: + color = motor_color(motor['model_name']) + print(f" Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") + change_motor_baudrate_only( + motor_type, motor['id'], port, motor['baud_rate'], new_baud + ) + else: + print(f"\n🔍 Waiting for motors to be connected at {current_baud:,} bps... (Ctrl+C to stop)") + known_ids = set() + while True: + client = create_config_client(motor_type, [], port, current_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[current_baud]) + found_ids = {m['id'] for m in motors} + new_motors = [m for m in motors if m['id'] not in known_ids] + + if new_motors: + for motor in new_motors: + color = motor_color(motor['model_name']) + print(f"\n Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") + change_motor_baudrate_only( + motor_type, motor['id'], port, motor['baud_rate'], new_baud + ) + print(f"\n🔍 Waiting for motors to be connected at {current_baud:,} bps... (Ctrl+C to stop)") + + known_ids = found_ids + time.sleep(1) + +def reset_loop(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int): + print(f"\n{BOLD}🔄 RESET MODE{RESET} — Scanning for configured motors to reset to factory defaults...") + print(f" Factory defaults: ID={default_id}, baudrate={default_baud:,}") + print(f" Press {BOLD}Ctrl+C{RESET} to stop.") + + if motor_type == 'feetech': + while True: + feetech_safe_connect_prompt(port, "Connect the motor(s) you want to reset") + print(f"\n🔍 Scanning for motors to reset...") + client = create_config_client(motor_type, [], port, target_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) + if not motors: + print(f"{YELLOW} No motors found. Check connections.{RESET}") + continue + for motor in motors: + color = motor_color(motor['model_name']) + print(f" Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") + reset_motor_to_factory( + motor_type, motor['id'], port, motor['baud_rate'], + default_id, default_baud + ) + else: + print(f"\n🔍 Waiting for a configured motor to be connected... (Ctrl+C to stop)") + known_ids = set() + while True: + client = create_config_client(motor_type, [], port, target_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) + found_ids = {m['id'] for m in motors} + new_motors = [m for m in motors if m['id'] not in known_ids] + + if new_motors: + for motor in new_motors: + color = motor_color(motor['model_name']) + print(f"\n Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") + reset_motor_to_factory( + motor_type, motor['id'], port, motor['baud_rate'], + default_id, default_baud + ) + print(f"\n🔍 Waiting for a configured motor to be connected... (Ctrl+C to stop)") + + known_ids = found_ids + time.sleep(1) + def main(): logging.basicConfig(level=logging.WARNING, format='%(levelname)s: %(message)s') - print("\n" + "=" * 60) - print(f"{BOLD}⚙️ ORCAHAND DYNAMIXEL CHAIN CONFIGURATION ⚙️{RESET}") - print() - + parser = argparse.ArgumentParser( - description="Configure Dynamixel motor chain for OrcaHand Assembly. Specify the path to the config file of your OrcaHand model.") + description="Configure motor chain for OrcaHand Assembly. Specify the path to the config file of your OrcaHand model.") parser.add_argument( "config_path", type=str, nargs="?", default=None, - help="Path to the hand config.yaml file.") + help="Path to config.yaml (e.g., orca_core/models/v2/orcahand_right/config.yaml). " + "Defaults to the bundled model when omitted.") + parser.add_argument( + "--reset", + action="store_true", + help="Reset configured motors back to factory defaults (runs in a loop until Ctrl+C)") + parser.add_argument( + "--baudrate", + type=int, + metavar="BAUD", + help="Change all motors to the specified baudrate without modifying IDs (runs in a loop until Ctrl+C)") + parser.add_argument( + "--motor-type", + type=str, + choices=list(MOTOR_TYPE_DEFAULTS.keys()), + help="Override motor family. Auto-detected from factory defaults " + "(Dynamixel @ 57600 / Feetech @ 1M, both ID 1) when omitted.") args = parser.parse_args() - + try: - config_path = os.path.abspath(args.config_path) if args.config_path else os.path.join(get_model_path(), "config.yaml") + if args.config_path is None: + config_path = os.path.join(get_model_path(), "config.yaml") + else: + config_path = os.path.abspath(args.config_path) + if not os.path.exists(config_path): + print(f"{RED}❌ config.yaml not found: {config_path}{RESET}") + return 1 config = read_yaml(config_path) - TARGET_BAUD = config.get('baudrate', 3000000) PORT = config.get('port', '/dev/ttyUSB0') motor_ids = config.get('motor_ids', list(range(17, 0, -1))) TOTAL_MOTORS = len(motor_ids) - if 'wrist' in config.get('joint_to_motor_map', {}): - XC430_ID = 1 - XC330_IDS = sorted([id for id in motor_ids if id != 1], reverse=True) # All except wrist ID, highest to lowest - else: - XC430_ID = None - XC330_IDS = sorted(motor_ids, reverse=True) # All motor IDs, highest to lowest except Exception as e: print(f"{RED}❌ Error loading config: {e}{RESET}") return 1 - + + # Pick a port before probing — auto-detect needs an actual device path. + PORT = validate_or_detect_port(PORT, config_path, args.motor_type or 'dynamixel') + + motor_type = args.motor_type + if motor_type is None: + print(f"\n{BOLD}Auto-detecting motor family on {PORT}...{RESET}") + motor_type = detect_motor_type(PORT) + if motor_type is None: + print(f"\n{RED}❌ No motors responded at either family's factory defaults.{RESET}") + print(" - Check that one motor is connected and powered.") + print(" - Confirm motors are still at factory defaults (re-runs of this script change them).") + print(" - Or pass --motor-type {dynamixel,feetech} to skip auto-detection.") + return 1 + print(f"{GREEN}✓ Detected {motor_type} motors.{RESET}\n") + + TARGET_BAUD = config.get('baudrate') or 1000000 + + defaults = MOTOR_TYPE_DEFAULTS[motor_type] + default_id = defaults['default_id'] + default_baud = defaults['default_baud'] + + motor_type_label = 'Dynamixel' if motor_type == 'dynamixel' else 'Feetech' + + if args.reset: + print("\n" + "=" * 60) + print(f"{BOLD}🔄 ORCAHAND {motor_type_label.upper()} CHAIN RESET 🔄{RESET}") + print("=" * 60) + try: + reset_loop(PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type, default_id, default_baud) + except KeyboardInterrupt: + print(f"\n\n{GREEN}Reset mode stopped.{RESET}\n") + return 0 + + if args.baudrate is not None: + valid_baudrates = get_valid_baudrates(motor_type) + if args.baudrate not in valid_baudrates: + print(f"{RED}❌ Invalid baudrate {args.baudrate:,} for {motor_type_label} motors{RESET}") + print(f"\n{YELLOW}Valid baudrates for {motor_type_label}:{RESET}") + for baud in valid_baudrates: + print(f" • {baud:,} bps") + return 1 + + print("\n" + "=" * 60) + print(f"{BOLD}🔄 ORCAHAND {motor_type_label.upper()} BAUDRATE CHANGE 🔄{RESET}") + print("=" * 60) + print(f"\nChanging all motors to baudrate: {BOLD}{args.baudrate:,} bps{RESET}") + print(f"Current baudrate in config: {BOLD}{TARGET_BAUD:,} bps{RESET}") + print(f"\n{YELLOW}Note: Motor IDs will NOT be changed, only baudrate.{RESET}") + try: + baudrate_change_loop(PORT, TARGET_BAUD, args.baudrate, TOTAL_MOTORS, motor_type) + except KeyboardInterrupt: + print(f"\n\n{GREEN}Baudrate change mode stopped.{RESET}\n") + return 0 + + print("\n" + "=" * 60) + print(f"{BOLD}⚙️ ORCAHAND {motor_type_label.upper()} CHAIN CONFIGURATION ⚙️{RESET}") + print() + + has_wrist = 'wrist' in config.get('joint_to_motor_map', {}) + if has_wrist: + WRIST_ID = 1 + FINGER_IDS = sorted([mid for mid in motor_ids if mid != 1], reverse=True) + else: + WRIST_ID = None + FINGER_IDS = sorted(motor_ids, reverse=True) + + if motor_type == 'dynamixel': + finger_model, wrist_model = 'XC330-T288-T', 'XC430-T240BB-T' + else: + finger_model, wrist_model = 'HLS3915', 'HLS3930' + print(f"\nFor the chosen hand model, we need to configure {TOTAL_MOTORS} motors in total:") - print(f" • {len(XC330_IDS)} {BLUE}XC330-T288-T{RESET} motors: {BOLD}IDs {min(XC330_IDS)}-{max(XC330_IDS)}{RESET} @ {TARGET_BAUD:,} bps") - if XC430_ID is not None: - print(f" • 1 {PURPLE}XC430-T240BB-T{RESET} motor: {BOLD}ID 1{RESET} @ {TARGET_BAUD:,} bps") + print(f" • {len(FINGER_IDS)} {BLUE}{finger_model}{RESET} motors: {BOLD}IDs {min(FINGER_IDS)}-{max(FINGER_IDS)}{RESET} @ {TARGET_BAUD:,} bps") + if WRIST_ID is not None: + print(f" • 1 {PURPLE}{wrist_model}{RESET} motor: {BOLD}ID 1{RESET} @ {TARGET_BAUD:,} bps") print("=" * 60) + all_target_ids = FINGER_IDS + ([WRIST_ID] if WRIST_ID is not None else []) + XC430_ID = WRIST_ID # back-compat: scan helpers still use the old name - already_configured = scan_already_configured_motors(PORT, TARGET_BAUD, TOTAL_MOTORS, XC430_ID).copy() + already_configured = scan_already_configured_motors(PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type, default_id, default_baud, XC430_ID).copy() configured_ids = already_configured.copy() - all_target_ids = XC330_IDS + ([XC430_ID] if XC430_ID is not None else []) remaining_ids = [id for id in all_target_ids if id not in already_configured] if remaining_ids: @@ -207,7 +587,7 @@ def main(): print(f"\n🔧 {BOLD}Troubleshooting guide:{RESET}") print(" ✓ Board connection: Properly connected to power and computer") print(" ✓ Motor connection: Properly wired to daisy chain") - print(" ✓ Motor settings: ID=1, baudrate=57600 (factory default)") + print(f" ✓ Motor settings: ID={default_id}, baudrate={default_baud:,} (factory default)") print(f" ✓ Serial port permissions (Linux): Your user must have read/write access to the serial port.") print(f" Run {BOLD}sudo usermod -aG dialout $USER{RESET} and re-login, or {BOLD}sudo chmod 666 /dev/ttyUSB0{RESET} for a quick fix.") print(f" ✓ Serial port path: Ensure {BOLD}config.yaml{RESET} has the correct port for your OS") @@ -220,34 +600,38 @@ def main(): print(f"🚀 {ORANGE}{BOLD}Ready for operation!{RESET}") print() return 0 - + for step, target_id in enumerate(remaining_ids, len(already_configured) + 1): - is_xc430 = (XC430_ID is not None and target_id == 1) - motor_type = 'XC430-T240BB-T' if is_xc430 else 'XC330-T288-T' - motor_color = BLUE if 'XC330' in motor_type else PURPLE if 'XC430' in motor_type else '' - + is_wrist = (WRIST_ID is not None and target_id == 1) + expected_model = wrist_model if is_wrist else finger_model + color = motor_color(expected_model) + if target_id == max(all_target_ids): - connection_msg = f"{ORANGE}Connect a {motor_color}{motor_type} {BOLD}(ID {target_id}){ORANGE} to the {BOLD}board{RESET}" + connection_msg = f"Connect a {color}{expected_model} {BOLD}(ID {target_id}){RESET}{ORANGE} to the {BOLD}board{RESET}" else: prev_id = target_id + 1 - connection_msg = f"{ORANGE}Connect a {motor_color}{motor_type} {BOLD}(ID {target_id}){RESET}{ORANGE} to the previous motor {BOLD}(ID {prev_id}){RESET}" - + connection_msg = f"Connect a {color}{expected_model} {BOLD}(ID {target_id}){RESET}{ORANGE} to the previous motor {BOLD}(ID {prev_id}){RESET}" + print(f"{UNDERLINE}{ORANGE}\nSTEP {step}/{TOTAL_MOTORS}{RESET}") - print(f"{connection_msg}") - print(f"\n🔍 Scanning for default {motor_color}{motor_type}{RESET} motor...") + + if motor_type == 'feetech': + feetech_safe_connect_prompt(PORT, connection_msg) + else: + print(f"{connection_msg}") + + print(f"\n🔍 Scanning for default {color}{expected_model}{RESET} motor...") try: while True: - if scan_for_default_motor(motor_type, PORT): - if configure_default_motor(target_id, PORT, TARGET_BAUD): + if scan_for_default_motor(expected_model, PORT, motor_type, default_id, default_baud): + if configure_default_motor(target_id, PORT, TARGET_BAUD, motor_type, default_id, default_baud): configured_ids.append(target_id) - if not verify_all_motors(configured_ids, PORT, TARGET_BAUD, TOTAL_MOTORS): + if not verify_all_motors(configured_ids, PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type): return 1 - break # Move to next motor + break else: return 1 else: - # Wait a bit before scanning again time.sleep(1) except Exception as e: print(f"\n ERROR: {e}") From fd8ed50a62ff3693d7098d0ee5db6d0109be2534 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Wed, 6 May 2026 16:47:02 +0200 Subject: [PATCH 04/10] Per-joint/finger calibration CLI + preserve old limits on interrupt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Calibrate part of the hand without disturbing the rest of an existing calibration, and stop a partial re-run from leaving the YAML in a half-state. scripts/calibrate.py - --fingers (thumb / index / middle / ring / pinky / wrist) and --joints (individual joint names) flags, mutually exclusive - Resolves the selection to a flat joint list and forwards via hand.calibrate(joints=...) orca_core/hardware_hand.py - _calibrate accumulates direction measurements in a separate pending_limits dict; only commits a joint's [lower, upper] to motor_limits once BOTH directions have been measured - Removes the upfront wipe loop that nulled motor_limits for every targeted joint before any motion. Previous behaviour was that an interrupted partial calibration zeroed out the joint's data even though the new values weren't measured yet — now the old values stay in motor_limits.yaml until the new ones are fully captured - _wrap_offsets_dict reset moved into the per-step setup so it still gets cleared for joints we touch this run Mirrors fb/dev's e80f7ad applied on top of our hardware_hand.py structure (we have CalibrationResult / config dataclass; fb/dev had flat attrs). --- orca_core/hardware_hand.py | 32 +++++++++++++++++------------ scripts/calibrate.py | 42 +++++++++++++++++++++++++++++++++++++- 2 files changed, 60 insertions(+), 14 deletions(-) diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index af62c4f4..65e847ce 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -659,22 +659,22 @@ def _calibrate( ) calibration_sequence = filtered - # Deep-copy current limits so per-step YAML writes reflect only the - # joints being calibrated, not stale data from a prior incomplete run. + # Deep-copy current limits. Joints not touched by this run keep their + # values; joints in this run accumulate their new limits in + # ``pending_limits`` and only overwrite ``motor_limits`` once both + # flex AND extend have been measured. That way an interrupted run + # never destroys a previously-good calibration. motor_limits = { motor_id: list(limits) for motor_id, limits in self.calibration.motor_limits_dict.items() } + pending_limits: Dict[int, list] = { + motor_id: [None, None] for motor_id in self.config.motor_ids + } joint_to_motor_ratios = dict(self.calibration.joint_to_motor_ratios_dict) self._compute_wrap_offsets_dict() - for step in calibration_sequence: - for joint in step[JOINTS].keys(): - motor_id = self.config.joint_to_motor_map[joint] - motor_limits[motor_id] = [None, None] - self._wrap_offsets_dict[motor_id] = 0.0 - motors_with_initial_offset = set() motors_with_final_offset = set() @@ -720,6 +720,7 @@ def _calibrate( sign = -sign directions[motor_id] = sign + self._wrap_offsets_dict[motor_id] = 0.0 position_buffers[motor_id] = deque( maxlen=self.config.calibration_num_stable ) @@ -775,9 +776,9 @@ def _calibrate( f"Motor {motor_id} corresponding to joint {self.config.motor_to_joint_dict[motor_id]} reached the limit at {avg_limit} rad." ) if directions[motor_id] == 1: - motor_limits[motor_id][1] = avg_limit + pending_limits[motor_id][1] = avg_limit if directions[motor_id] == -1: - motor_limits[motor_id][0] = avg_limit + pending_limits[motor_id][0] = avg_limit if ( self._motor_client.requires_offset_calibration @@ -789,7 +790,7 @@ def _calibrate( ) time.sleep(TINY_SLEEP) new_limit = float(self.get_motor_pos()[idx]) - motor_limits[motor_id][1 if is_positive else 0] = ( + pending_limits[motor_id][1 if is_positive else 0] = ( new_limit ) print( @@ -802,11 +803,16 @@ def _calibrate( for joint in step[JOINTS].keys(): motor_id = self.config.joint_to_motor_map[joint] if ( - motor_limits[motor_id][0] is None - or motor_limits[motor_id][1] is None + pending_limits[motor_id][0] is None + or pending_limits[motor_id][1] is None ): + # Only one direction has completed for this joint; leave + # motor_limits alone so an interrupted run doesn't lose + # the previously-known good values. continue + # Both directions measured — commit to motor_limits. + motor_limits[motor_id] = list(pending_limits[motor_id]) delta_motor = motor_limits[motor_id][1] - motor_limits[motor_id][0] delta_joint = ( self.config.joint_roms_dict[joint][1] diff --git a/scripts/calibrate.py b/scripts/calibrate.py index 0658340f..9d839fe6 100644 --- a/scripts/calibrate.py +++ b/scripts/calibrate.py @@ -5,6 +5,18 @@ from common import add_hand_arguments, connect_hand, create_hand, shutdown_hand +FINGER_TO_JOINTS = { + "thumb": ["thumb_cmc", "thumb_abd", "thumb_mcp", "thumb_dip"], + "index": ["index_abd", "index_mcp", "index_pip"], + "middle": ["middle_abd", "middle_mcp", "middle_pip"], + "ring": ["ring_abd", "ring_mcp", "ring_pip"], + "pinky": ["pinky_abd", "pinky_mcp", "pinky_pip"], + "wrist": ["wrist"], +} + +ALL_JOINTS = [j for joints in FINGER_TO_JOINTS.values() for j in joints] + + def main() -> int: parser = argparse.ArgumentParser( description="Run the ORCA hand calibration routine." @@ -15,13 +27,41 @@ def main() -> int: action="store_true", help="Recalibrate the wrist even if it is already marked as calibrated.", ) + parser.add_argument( + "--fingers", + type=str, + nargs="+", + choices=list(FINGER_TO_JOINTS.keys()), + help="Fingers to calibrate (e.g., --fingers thumb index pinky)", + ) + parser.add_argument( + "--joints", + type=str, + nargs="+", + choices=ALL_JOINTS, + help="Individual joints to calibrate (e.g., --joints thumb_cmc index_mcp)", + ) args = parser.parse_args() + if args.fingers and args.joints: + parser.error("Cannot specify both --fingers and --joints. Use one or the other.") + + joints = None + if args.fingers: + joints = [] + for finger in args.fingers: + joints.extend(FINGER_TO_JOINTS[finger]) + print(f"Calibrating fingers: {args.fingers}") + print(f"Resolved joints: {joints}") + elif args.joints: + joints = args.joints + print(f"Calibrating joints: {joints}") + hand = create_hand(args.config_path, use_mock=args.mock) try: connect_hand(hand) print("Starting calibration...") - hand.calibrate(force_wrist=args.force_wrist) + hand.calibrate(force_wrist=args.force_wrist, joints=joints) print("Calibration complete.") return 0 finally: From 184d72c1d2406a75de12bb6030089e3a871be442 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Thu, 7 May 2026 12:18:32 +0200 Subject: [PATCH 05/10] Per-motor read fallback for Dynamixel + auto-calibrate in stress_test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit DynamixelClient - Mirror the sync-with-fallback pattern Feetech got in 1df95b3. DynamixelReader.read() now falls back to per-motor reads on bulk failure (entire packet) or on partial response (specific IDs missing from the bulk packet) - Default _read_per_motor_fallback is a no-op so subclasses without an override (e.g. _moving_status_reader) keep their stale-cache behaviour, matching the prior version - DynamixelPosVelCurReader._read_per_motor_fallback uses read4ByteTxRx for position/velocity and read2ByteTxRx for current, same unsigned_to_signed + scale conversion as the bulk path. A motor whose individual read also fails keeps its previously-cached value - DynamixelTempReader._read_per_motor_fallback does the same with read1ByteTxRx for ADDR_PRESENT_TEMPERATURE - Resolves Francesco's PR #62 L276 comment ("do the same in dynamixel so both clients are on par"). One bad cable / dropped motor on the daisy chain no longer blanks every joint reading on screen — only the actual offender shows stale data stress_test.py - Switch hand.enable_torque() -> hand.init_joints(force_calibrate= args.mock). init_joints handles enable-torque, control-mode setup, AND auto-calibration if the hand isn't calibrated yet, matching every other position-commanding script in scripts/. Running stress_test on a fresh hand will now run the full calibration routine first instead of commanding undefined joint positions --- orca_core/hardware/dynamixel_client.py | 93 +++++++++++++++++++++++--- scripts/stress_test.py | 2 +- 2 files changed, 86 insertions(+), 9 deletions(-) diff --git a/orca_core/hardware/dynamixel_client.py b/orca_core/hardware/dynamixel_client.py index 337740c5..9417d41d 100644 --- a/orca_core/hardware/dynamixel_client.py +++ b/orca_core/hardware/dynamixel_client.py @@ -668,7 +668,7 @@ def __init__(self, client: DynamixelClient, motor_ids: Sequence[int], .format(motor_id)) def read(self, retries: int = 1): - """Reads data from the motors.""" + """Read data via bulk; fall back to per-motor reads on failure.""" self.client.check_connected() success = False while not success and retries >= 0: @@ -677,8 +677,12 @@ def read(self, retries: int = 1): comm_result, context='read') retries -= 1 - # If we failed, send a copy of the previous data. if not success: + logging.warning( + 'Bulk read failed; falling back to per-motor reads for %d motor(s)', + len(self.motor_ids), + ) + self._read_per_motor_fallback(list(self.motor_ids)) return self._get_data() # Check for Alert bits in the status packets we already received. @@ -686,28 +690,34 @@ def read(self, retries: int = 1): if error & _AlertCaptureBulkRead.ALERT_BIT: self.client._handle_hardware_alert(motor_id) - errored_ids = [] + missing_ids = [] for i, motor_id in enumerate(self.motor_ids): # Check if the data is available. available = self.operation.isAvailable(motor_id, self.address, self.size) if not available: - errored_ids.append(motor_id) + missing_ids.append(motor_id) continue try: self._update_data(i, motor_id) except Exception as e: logging.error(f'Error updating data for motor {motor_id}: {e}') - errored_ids.append(motor_id) + missing_ids.append(motor_id) continue - if errored_ids: - logging.error('Bulk read data is unavailable for: %s', - str(errored_ids)) + if missing_ids: + logging.warning( + 'Bulk read missing data for %s; per-motor fallback', + missing_ids, + ) + self._read_per_motor_fallback(missing_ids) return self._get_data() + def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None: + """Read each motor in ``motor_ids`` individually.""" + def _initialize_data(self): """Initializes the cached data.""" self._data = np.zeros(len(self.motor_ids), dtype=np.float32) @@ -762,6 +772,50 @@ def _update_data(self, index: int, motor_id: int): self._vel_data[index] = float(vel) * self.vel_scale self._cur_data[index] = float(cur) * self.cur_scale + def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None: + """Per-motor reads for position / velocity / current. + + Used when the bulk read fails entirely or when specific motor IDs + weren't included in the bulk packet. Cached values are kept for + motors whose individual read also fails. + """ + port = self.client.port_handler + packet = self.client.packet_handler + comm_success = self.client.dxl.COMM_SUCCESS + for motor_id in motor_ids: + try: + idx = self.motor_ids.index(motor_id) + except ValueError: + continue + try: + pos_raw, comm, _ = packet.read4ByteTxRx( + port, motor_id, ADDR_PRESENT_POSITION + ) + if comm == comm_success: + self._pos_data[idx] = ( + float(unsigned_to_signed(pos_raw, size=4)) * self.pos_scale + ) + vel_raw, comm, _ = packet.read4ByteTxRx( + port, motor_id, ADDR_PRESENT_VELOCITY + ) + if comm == comm_success: + self._vel_data[idx] = ( + float(unsigned_to_signed(vel_raw, size=4)) * self.vel_scale + ) + cur_raw, comm, _ = packet.read2ByteTxRx( + port, motor_id, ADDR_PRESENT_CURRENT + ) + if comm == comm_success: + self._cur_data[idx] = ( + float(unsigned_to_signed(cur_raw, size=2)) * self.cur_scale + ) + except Exception as e: + logging.warning( + 'Per-motor pos/vel/cur read failed for motor %d: %s', + motor_id, + e, + ) + def _get_data(self): """Returns a copy of the data.""" return (self._pos_data.copy(), self._vel_data.copy(), @@ -779,6 +833,29 @@ def _update_data(self, index: int, motor_id: int): raw_val = self.operation.getData(motor_id, self.address, self.size) self._temp_data[index] = float(raw_val) + def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None: + """Per-motor temperature reads.""" + port = self.client.port_handler + packet = self.client.packet_handler + comm_success = self.client.dxl.COMM_SUCCESS + for motor_id in motor_ids: + try: + idx = self.motor_ids.index(motor_id) + except ValueError: + continue + try: + raw, comm, _ = packet.read1ByteTxRx( + port, motor_id, ADDR_PRESENT_TEMPERATURE + ) + if comm == comm_success: + self._temp_data[idx] = float(raw) + except Exception as e: + logging.warning( + 'Per-motor temperature read failed for motor %d: %s', + motor_id, + e, + ) + def _get_data(self): return self._temp_data.copy() diff --git a/scripts/stress_test.py b/scripts/stress_test.py index 8fce015f..f2b1bf4b 100644 --- a/scripts/stress_test.py +++ b/scripts/stress_test.py @@ -126,7 +126,7 @@ def main() -> int: hand = create_hand(args.config_path, use_mock=args.mock) try: connect_hand(hand) - hand.enable_torque() + hand.init_joints(force_calibrate=args.mock) last_temp_check = 0.0 try: From 9d35262836680c78a05e161f84ec72510a55a90a Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 12 May 2026 10:42:00 +0200 Subject: [PATCH 06/10] Address PR #64 review: MotorRead dataclass, clean configure_motor_chain MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Round of cleanups in response to Francesco's review comments on PR #64. API - New MotorRead NamedTuple (position / velocity / current) returned by read_position_velocity_current (renamed from read_pos_vel_cur). Named attribute access AND tuple unpacking so callers can keep doing `pos, vel, cur = client.read_position_velocity_current()`. Migrated every caller in hardware_hand, mock client, dynamixel demo block, and the three diagnostic scripts. dynamixel_client - dynamixel_sdk import hoisted to module level (PortHandler / PacketHandler / COMM_SUCCESS) so probe() doesn't pay name-resolution per call. Test fakes in conftest.py and scripts/common.py extended with COMM_SUCCESS. - _read_per_motor_fallback hook docstring made explicit so the template-method pattern isn't misread as duplicate dead code. feetech_client - FEETECH_MODELS comment trimmed to one line. - scan_for_motors signature uses Optional[list] = None with an explicit `if baud_rates is None` check rather than the truthy-fallback idiom. - Brief inline comment on the position clamp in write_positions_sync. calibrate.py - Mutex error message wording: "Use either one." per Francesco's suggestion. configure_motor_chain.py (significant rewrite) - Top-level FEETECH/DYNAMIXEL/MOTOR_TYPE_DEFAULTS/MOTOR_MODELS constants replace scattered string literals. - `mid` renamed to `motor_id` throughout. - print() routed through `logger.error` / `logger.info` for error and status messages so users can silence them via logging config. - get_valid_baudrates gets explicit elif/else with ValueError. - AI-slop comments trimmed; docstrings added for scan_already_configured_motors and the obscure id-walk + expected-sequence prints inside it. - Two near-identical port-wait helpers collapsed into one (`present=` kwarg). - Two near-identical bulk loops (baudrate change vs factory reset) collapsed into _process_motors_loop with an action callback. - Triple `client = ...; client.connect(); ...; _close_silently(client)` pattern replaced by a _config_session context manager. - Per-family branching (`if motor_type == FEETECH` etc.) replaced by MOTOR_TYPE_LABELS / MOTOR_MODELS / VALID_BAUDRATES dict lookups. - Decorative section dividers and unused colour constants dropped. - Net: 701 -> 544 lines (~22% smaller) with the actual duplication gone. yaml configs - Inline port/baudrate "uncomment to override" hints removed across all bundled configs — Francesco's "ship batteries-included" point. Override documentation goes in docs, not in every shipped config. tests - fake_serial_port + patch_comports + mock_hand fixtures moved from test_connect_resolution.py into conftest.py so test files stay focused. - Hop-over helper killed: tests now call OrcaHand._trial_probe(hand, port) directly to exercise the real implementation on a MockOrcaHand instance. --- orca_core/hardware/dynamixel_client.py | 21 +- orca_core/hardware/feetech_client.py | 21 +- orca_core/hardware/mock_dynamixel_client.py | 12 +- orca_core/hardware/motor_client.py | 22 +- orca_core/hardware_hand.py | 4 +- orca_core/models/v1/orcahand_left/config.yaml | 2 - .../models/v1/orcahand_right/config.yaml | 2 - orca_core/models/v2/orcahand_left/config.yaml | 2 - .../models/v2/orcahand_touch_left/config.yaml | 2 - .../v2/orcahand_touch_right/config.yaml | 2 - scripts/calibrate.py | 2 +- scripts/check_motor.py | 2 +- scripts/common.py | 1 + scripts/configure_motor_chain.py | 827 ++++++++---------- scripts/debug_overload.py | 2 +- scripts/test_overload.py | 2 +- tests/conftest.py | 22 + tests/test_connect_resolution.py | 64 +- 18 files changed, 454 insertions(+), 558 deletions(-) diff --git a/orca_core/hardware/dynamixel_client.py b/orca_core/hardware/dynamixel_client.py index 9417d41d..deccb5e7 100644 --- a/orca_core/hardware/dynamixel_client.py +++ b/orca_core/hardware/dynamixel_client.py @@ -20,7 +20,13 @@ from typing import Optional, Sequence, Union, Tuple import numpy as np -from .motor_client import MotorClient +from dynamixel_sdk import ( + COMM_SUCCESS, + PacketHandler, + PortHandler, +) + +from .motor_client import MotorClient, MotorRead PROTOCOL_VERSION = 2.0 @@ -227,8 +233,6 @@ def probe( Dynamixel Protocol 2.0 at this baudrate. Used at connect time to auto-detect the driver family without enabling torque. """ - from dynamixel_sdk import PortHandler, PacketHandler, COMM_SUCCESS - handler = PortHandler(port) try: if not handler.openPort(): @@ -306,13 +310,14 @@ def set_operating_mode(self, motor_ids: Sequence[int], mode_value: int): for mid in motor_ids: self._operating_modes[mid] = mode_value - def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Returns the positions, velocities, and currents. + def read_position_velocity_current(self) -> MotorRead: + """Return positions, velocities, and currents as a ``MotorRead`` snapshot. Overload detection is handled reactively via the Alert bit in handle_packet_result, so no extra bulk read is needed here. """ - return self._pos_vel_cur_reader.read() + pos, vel, cur = self._pos_vel_cur_reader.read() + return MotorRead(position=pos, velocity=vel, current=cur) def read_status_is_done_moving(self) -> bool: """Returns the last bit of moving status""" @@ -716,7 +721,7 @@ def read(self, retries: int = 1): return self._get_data() def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None: - """Read each motor in ``motor_ids`` individually.""" + """Hook overridden by subclasses to read each motor individually. Default no-op keeps cached values.""" def _initialize_data(self): """Initializes the cached data.""" @@ -892,7 +897,7 @@ def _get_data(self): print('Writing: {}'.format(way_point.tolist())) dxl_client.write_desired_pos(motors, way_point) read_start = time.time() - pos_now, vel_now, cur_now = dxl_client.read_pos_vel_cur() + pos_now, vel_now, cur_now = dxl_client.read_position_velocity_current() if step % 5 == 0: print('[{}] Frequency: {:.2f} Hz'.format( step, 1.0 / (time.time() - read_start))) diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index ba9ade10..13799e9f 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -14,7 +14,7 @@ from typing import Optional, Sequence, Tuple import numpy as np -from .motor_client import MotionTimeoutError, MotorClient +from .motor_client import MotionTimeoutError, MotorClient, MotorRead from .feetech import ( PortHandler, sms_sts, @@ -48,10 +48,7 @@ 38_400: 7, } -# Model-number → human name. The string is used by configure_motor_chain -# for substring-matching, so multiple raw model numbers can share a label -# (e.g., HLS3915 ships in two firmware revisions reporting different -# model_number values; both are functionally HLS3915). +# Model-number → human name. Multiple raw values can map to the same label. FEETECH_MODELS: dict[int, str] = { 4106: 'HLS3930', 6922: 'HLS3915', @@ -201,7 +198,7 @@ def disconnect(self) -> None: def scan_for_motors( port: str = '/dev/ttyUSB0', id_range: tuple = (0, 252), - baud_rates: list = None, + baud_rates: Optional[list] = None, ) -> list: """Scan ``port`` for any responding Feetech motors. @@ -209,7 +206,8 @@ def scan_for_motors( every ID in ``id_range``, and returns a list of dicts with keys ``id``, ``baud_rate``, ``model_name``. Doesn't change motor state. """ - baud_rates = baud_rates or list(FEETECH_BAUD_RATE_MAP.keys()) + if baud_rates is None: + baud_rates = list(FEETECH_BAUD_RATE_MAP.keys()) detected_motors = [] for baud_rate in baud_rates: port_handler = PortHandler(port) @@ -370,7 +368,7 @@ def set_operating_mode(self, motor_ids: Sequence[int], mode: int) -> None: # Re-enable torque self.set_torque_enabled(motor_ids, True) - def _read_state_per_motor_fallback(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + def _read_state_per_motor_fallback(self) -> MotorRead: """Per-motor read of position/velocity/current; used only when sync read fails.""" self._check_connected() @@ -419,7 +417,7 @@ def _read_state_per_motor_fallback(self) -> Tuple[np.ndarray, np.ndarray, np.nda motor_id, result, error ) - return positions, velocities, currents + return MotorRead(position=positions, velocity=velocities, current=currents) def read_temperature(self) -> np.ndarray: """Reads the temperature for all motors via a single sync-read packet.""" @@ -708,6 +706,7 @@ def write_positions_sync( self.packet_handler.groupSyncWrite.clearParam() for motor_id, pos_rad in zip(motor_ids, positions): + # STS servo mode uses raw 0–4095 (single rotation); clamp before sending. pos_raw = self._clamp_position(self._rad_to_raw(pos_rad, self.pos_scale)) logging.debug( @@ -725,7 +724,7 @@ def write_positions_sync( # Clear params for next use self.packet_handler.groupSyncWrite.clearParam() - def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + def read_position_velocity_current(self) -> MotorRead: """Read position, velocity, and current for all motors in one sync packet. Falls back to per-motor reads only if the sync transaction fails. @@ -768,7 +767,7 @@ def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: else: logging.warning('Motor %d not available in sync read', motor_id) - return positions, velocities, currents + return MotorRead(position=positions, velocity=velocities, current=currents) # Register global cleanup function diff --git a/orca_core/hardware/mock_dynamixel_client.py b/orca_core/hardware/mock_dynamixel_client.py index 7c8480ca..5fc69e5d 100644 --- a/orca_core/hardware/mock_dynamixel_client.py +++ b/orca_core/hardware/mock_dynamixel_client.py @@ -15,7 +15,7 @@ from typing import Optional, Sequence, Union, Tuple import numpy as np -from .motor_client import MotorClient +from .motor_client import MotorClient, MotorRead PROTOCOL_VERSION = 2.0 @@ -218,14 +218,14 @@ def set_operating_mode(self, motor_ids: Sequence[int], mode_value: int): logging.info('Set operating mode for motor %d to %d', mid, mode_value) - def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: + def read_position_velocity_current(self) -> MotorRead: """Returns the simulated positions, velocities, and currents.""" - + pos_array = np.array([self._pos[mid] for mid in self.motor_ids]) vel_array = np.array([self._vel[mid] for mid in self.motor_ids]) cur_array = np.array([self._cur[mid] for mid in self.motor_ids]) - - return pos_array, vel_array, cur_array + + return MotorRead(position=pos_array, velocity=vel_array, current=cur_array) def read_status_is_done_moving(self) -> bool: """Returns the last bit of moving status""" @@ -513,7 +513,7 @@ def _get_data(self): print('Writing: {}'.format(way_point.tolist())) dxl_client.write_desired_pos(motors, way_point) read_start = time.time() - pos_now, vel_now, cur_now = dxl_client.read_pos_vel_cur() + pos_now, vel_now, cur_now = dxl_client.read_position_velocity_current() if step % 5 == 0: print('[{}] Frequency: {:.2f} Hz'.format( step, 1.0 / (time.time() - read_start))) diff --git a/orca_core/hardware/motor_client.py b/orca_core/hardware/motor_client.py index 2986e1b1..4cea2feb 100644 --- a/orca_core/hardware/motor_client.py +++ b/orca_core/hardware/motor_client.py @@ -9,7 +9,7 @@ """Abstract base class for motor communication clients.""" from abc import ABC, abstractmethod -from typing import Sequence, Tuple +from typing import NamedTuple, Sequence import numpy as np @@ -21,6 +21,18 @@ class MotionTimeoutError(MotorError): """Raised when motors fail to settle within the requested timeout.""" +class MotorRead(NamedTuple): + """A single snapshot of position / velocity / current for all motors. + + Each field is a 1-D numpy array indexed by the motor order configured + on the client. NamedTuple so callers can also unpack as + ``position, velocity, current = client.read_position_velocity_current()``. + """ + position: np.ndarray + velocity: np.ndarray + current: np.ndarray + + class MotorClient(ABC): """Abstract base class for motor communication clients. @@ -82,12 +94,12 @@ def set_operating_mode(self, motor_ids: Sequence[int], mode: int) -> None: ... @abstractmethod - def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: - """Reads the current position, velocity, and current for all motors. + def read_position_velocity_current(self) -> MotorRead: + """Read the current position, velocity, and current for all motors. Returns: - A tuple of (positions, velocities, currents) as numpy arrays. - Positions are in radians, velocities in rad/s, currents in mA. + A :class:`MotorRead` snapshot. Positions are in radians, + velocities in rad/s, currents in mA. """ ... diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 65e847ce..971e2bda 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -394,7 +394,7 @@ def get_motor_pos(self, as_dict: bool = False) -> Union[np.ndarray, dict]: Motor positions in radians as an array or dict. """ with self._motor_lock: - motor_pos = self._motor_client.read_pos_vel_cur()[0] + motor_pos = self._motor_client.read_position_velocity_current().position if as_dict: return { @@ -414,7 +414,7 @@ def get_motor_current(self, as_dict: bool = False) -> Union[np.ndarray, dict]: Motor currents (mA) as an array, or dict. """ with self._motor_lock: - motor_current = self._motor_client.read_pos_vel_cur()[2] + motor_current = self._motor_client.read_position_velocity_current().current if as_dict: return { diff --git a/orca_core/models/v1/orcahand_left/config.yaml b/orca_core/models/v1/orcahand_left/config.yaml index 77cb9fcc..17a3886f 100644 --- a/orca_core/models/v1/orcahand_left/config.yaml +++ b/orca_core/models/v1/orcahand_left/config.yaml @@ -1,5 +1,3 @@ -# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) -# baudrate: 3000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 400 type: right diff --git a/orca_core/models/v1/orcahand_right/config.yaml b/orca_core/models/v1/orcahand_right/config.yaml index ef07d27d..34b700d4 100644 --- a/orca_core/models/v1/orcahand_right/config.yaml +++ b/orca_core/models/v1/orcahand_right/config.yaml @@ -1,5 +1,3 @@ -# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) -# baudrate: 3000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 400 diff --git a/orca_core/models/v2/orcahand_left/config.yaml b/orca_core/models/v2/orcahand_left/config.yaml index 40eff86b..d5de5645 100644 --- a/orca_core/models/v2/orcahand_left/config.yaml +++ b/orca_core/models/v2/orcahand_left/config.yaml @@ -1,5 +1,3 @@ -# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) -# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: left diff --git a/orca_core/models/v2/orcahand_touch_left/config.yaml b/orca_core/models/v2/orcahand_touch_left/config.yaml index 10c3cc8c..82d6cdc8 100644 --- a/orca_core/models/v2/orcahand_touch_left/config.yaml +++ b/orca_core/models/v2/orcahand_touch_left/config.yaml @@ -1,5 +1,3 @@ -# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) -# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: left diff --git a/orca_core/models/v2/orcahand_touch_right/config.yaml b/orca_core/models/v2/orcahand_touch_right/config.yaml index 64a45d0c..a673c456 100644 --- a/orca_core/models/v2/orcahand_touch_right/config.yaml +++ b/orca_core/models/v2/orcahand_touch_right/config.yaml @@ -1,5 +1,3 @@ -# port: /dev/cu.usbmodemXXXX # Uncomment to override auto-detection (e.g. multiple hands plugged in) -# baudrate: 1000000 # Uncomment to override default baudrate (e.g. custom hand) version: 0.2.1 max_current: 300 type: right diff --git a/scripts/calibrate.py b/scripts/calibrate.py index 9d839fe6..b382e652 100644 --- a/scripts/calibrate.py +++ b/scripts/calibrate.py @@ -44,7 +44,7 @@ def main() -> int: args = parser.parse_args() if args.fingers and args.joints: - parser.error("Cannot specify both --fingers and --joints. Use one or the other.") + parser.error("Cannot specify both --fingers and --joints. Use either one.") joints = None if args.fingers: diff --git a/scripts/check_motor.py b/scripts/check_motor.py index fe2d744c..1d3d7e66 100644 --- a/scripts/check_motor.py +++ b/scripts/check_motor.py @@ -43,7 +43,7 @@ def main(): # Added main function try: while True: - pos = dxl_client.read_pos_vel_cur()[0] + pos = dxl_client.read_position_velocity_current()[0] increment = -0.1 if args.reverse else 0.1 new_pos = pos + increment dxl_client.write_desired_pos([args.motor_id], new_pos) diff --git a/scripts/common.py b/scripts/common.py index 9a691d16..0e824528 100644 --- a/scripts/common.py +++ b/scripts/common.py @@ -48,6 +48,7 @@ def getData(self, motor_id: int, address: int, size: int) -> int: module.PortHandler = PortHandler module.PacketHandler = PacketHandler module.GroupBulkRead = GroupBulkRead + module.COMM_SUCCESS = 0 sys.modules["dynamixel_sdk"] = module diff --git a/scripts/configure_motor_chain.py b/scripts/configure_motor_chain.py index 1c45f28f..04e451cb 100755 --- a/scripts/configure_motor_chain.py +++ b/scripts/configure_motor_chain.py @@ -1,135 +1,150 @@ -import sys -import time -import logging import argparse +import contextlib +import logging import os import subprocess +import sys +import time sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from orca_core.hardware.dynamixel_client import DynamixelClient, BAUD_RATE_MAP as DYNAMIXEL_BAUD_RATE_MAP from orca_core.hardware.feetech_client import FeetechClient, FEETECH_BAUD_RATE_MAP -from orca_core.utils import read_yaml, get_model_path, auto_detect_port, get_and_choose_port - -RED = '\033[91m' -GREEN = '\033[92m' -BLUE = '\033[94m' -PURPLE = '\033[95m' -YELLOW = '\033[93m' -ORANGE = '\033[38;5;208m' -BOLD = '\033[1m' -UNDERLINE = '\033[4m' -RESET = '\033[0m' - -def validate_or_detect_port(port: str, config_path: str, motor_type: str) -> str: - """Check if port exists, auto-detect or prompt if not.""" - if os.path.exists(port): - return port +from orca_core.utils import auto_detect_port, get_and_choose_port, get_model_path, read_yaml - print(f"{YELLOW}⚠ Port {port} not found.{RESET}") - detected = auto_detect_port(motor_type) - if detected and os.path.exists(detected): - print(f"{GREEN}✓ Using auto-detected port: {detected}{RESET}") - return detected +RED, GREEN, BLUE, PURPLE, YELLOW = '\033[91m', '\033[92m', '\033[94m', '\033[95m', '\033[93m' +ORANGE, BOLD, RESET = '\033[38;5;208m', '\033[1m', '\033[0m' - print("Please select a port from available devices:") - chosen = get_and_choose_port() - if chosen: - print(f"{GREEN}✓ Using selected port: {chosen}{RESET}") - return chosen +FEETECH, DYNAMIXEL = 'feetech', 'dynamixel' - print(f"{RED}❌ No valid port found. Check your USB connection.{RESET}") - sys.exit(1) +# Per-family lookup tables. Index by motor_type instead of branching. +MOTOR_TYPE_DEFAULTS = { + DYNAMIXEL: {'default_id': 1, 'default_baud': 57600}, + FEETECH: {'default_id': 1, 'default_baud': 1000000}, +} +MOTOR_MODELS = { + DYNAMIXEL: {'wrist': 'XC430', 'finger': 'XC330'}, + FEETECH: {'wrist': 'HLS3930', 'finger': 'HLS3915'}, +} +logger = logging.getLogger(__name__) -def wait_for_port_removed(port: str, timeout: float = 30): - """Wait until the serial port disappears (USB unplugged).""" - print(f"\n{YELLOW}⚠ REMOVE POWER:{RESET} Unplug the USB cable from your computer.") - print(f" Waiting for {BOLD}{port}{RESET} to disappear...") - start = time.time() - while os.path.exists(port): - if time.time() - start > timeout: - print(f"{RED}❌ Timeout: port {port} still exists. Unplug the USB cable.{RESET}") - start = time.time() - time.sleep(0.3) - print(f"{GREEN}✓ USB disconnected.{RESET}") +# --- UI helpers ------------------------------------------------------------- + +def validate_or_detect_port(port: str, motor_type: str) -> str: + """Return ``port`` if present; else auto-detect or prompt; else exit.""" + if os.path.exists(port): + return port + print(f"{YELLOW}⚠ Port {port} not found.{RESET}") + for candidate, source in ( + (auto_detect_port(motor_type), "auto-detected"), + (get_and_choose_port(), "selected"), + ): + if candidate and os.path.exists(candidate): + print(f"{GREEN}✓ Using {source} port: {candidate}{RESET}") + return candidate + print(f"{RED}❌ No valid port found. Check your USB connection.{RESET}") + sys.exit(1) -def wait_for_port_reconnected(port: str, timeout: float = 60): - """Wait until the serial port reappears (USB plugged back in).""" - print(f"\n{YELLOW}▶ RESTORE POWER:{RESET} Plug the USB cable back in.") - print(f" Waiting for {BOLD}{port}{RESET} to reappear...") + +def wait_for_port(port: str, *, present: bool, timeout: float = 30): + """Block until ``port`` appears (present=True) or disappears (present=False).""" + if present: + print(f"\n{YELLOW}▶ RESTORE POWER:{RESET} Plug the USB cable back in.") + else: + print(f"\n{YELLOW}⚠ REMOVE POWER:{RESET} Unplug the USB cable from your computer.") start = time.time() - while not os.path.exists(port): + while os.path.exists(port) != present: if time.time() - start > timeout: - print(f"{RED}❌ Timeout: port {port} not found. Plug in the USB cable.{RESET}") + print(f"{RED}❌ Timeout waiting for {port}.{RESET}") start = time.time() time.sleep(0.3) - time.sleep(0.5) - print(f"{GREEN}✓ USB reconnected on {port}.{RESET}") + if present: + time.sleep(0.5) # let the OS finish bringing the device up + print(f"{GREEN}✓ USB {'re' if present else 'dis'}connected.{RESET}") def feetech_safe_connect_prompt(port: str, connection_msg: str): - """Guide user through safe power-off motor connection for Feetech servos.""" - wait_for_port_removed(port) + """Power-cycle dance: USB out → user plugs motor → USB back in.""" + wait_for_port(port, present=False) print(f"\n{ORANGE}🔌 {connection_msg}{RESET}") input(f"\n Press {BOLD}Enter{RESET} when the motor is connected correctly...") - wait_for_port_reconnected(port) + wait_for_port(port, present=True) -MOTOR_TYPE_DEFAULTS = { - 'dynamixel': {'default_id': 1, 'default_baud': 57600}, - 'feetech': {'default_id': 1, 'default_baud': 1000000}, -} +def play_success_beep(): + """Best-effort audible cue when a motor finishes configuration.""" + for cmd in ( + ['paplay', '/usr/share/sounds/freedesktop/stereo/complete.oga'], + ['beep', '-f', '800', '-l', '100'], + ): + try: + subprocess.run(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) + return + except Exception: + continue + print('\a', end='', flush=True) + + +def motor_color(model_name: str) -> str: + """ANSI colour for a motor model: finger→blue, wrist→purple.""" + for family in MOTOR_MODELS.values(): + if family['finger'] in model_name: + return BLUE + if family['wrist'] in model_name: + return PURPLE + return '' + + +def _print_motor_row(motor: dict) -> None: + color = motor_color(motor['model_name']) + print(f" • ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") + + +def _banner(title: str) -> None: + print("\n" + "=" * 60) + print(f"{BOLD}{title}{RESET}") + print("=" * 60) + -def create_config_client(motor_type, motor_ids, port, baudrate): - if motor_type == 'feetech': - return FeetechClient(motor_ids, port=port, baudrate=baudrate) - return DynamixelClient(motor_ids, port=port, baudrate=baudrate) +# --- Motor client helpers --------------------------------------------------- +def _make_client(motor_type, motor_ids, port, baudrate): + cls = FeetechClient if motor_type == FEETECH else DynamixelClient + return cls(motor_ids, port=port, baudrate=baudrate) -def _close_silently(client) -> None: - """Close the port without sending torque-disable. - During motor (re)configuration the motor's bus ID or baudrate changes - mid-session, so a normal ``client.disconnect()`` would fire a torque- - disable packet at an address the motor no longer answers on, producing - spurious "no status packet" errors. Closing the port directly avoids - the failed write while still releasing the OS handle. +@contextlib.contextmanager +def _config_session(motor_type, motor_ids, port, baudrate): + """Connect a client, yield it, close the port directly on exit. + + Bypasses the normal disconnect-with-torque-disable because callers may + have just changed the motor's ID or baudrate mid-session. """ + client = _make_client(motor_type, motor_ids, port, baudrate) + client.connect() try: - client.port_handler.closePort() - except Exception: - pass + yield client + finally: + try: + client.port_handler.closePort() + except Exception: + pass def detect_motor_type(port: str) -> str | None: """Probe ``port`` at each family's factory defaults to identify motors. Fresh motors ship at known (baud, ID) pairs that differ between families: - Dynamixel @ 57600 ID 1, Feetech @ 1M ID 1. Pinging at these defaults - tells us which protocol the bus speaks without needing user input. - - Returns ``"dynamixel"``, ``"feetech"``, or ``None`` when nothing responds - (no motor connected, motor not at factory defaults, or wiring issue). + Dynamixel @ 57600 ID 1, Feetech @ 1M ID 1. """ for motor_type, defaults in MOTOR_TYPE_DEFAULTS.items(): - baud = defaults['default_baud'] - default_id = defaults['default_id'] - print( - f" Probing {motor_type} at factory defaults " - f"(ID {default_id} @ {baud} baud)...", - end=' ', - flush=True, - ) + baud, default_id = defaults['default_baud'], defaults['default_id'] + print(f" Probing {motor_type} (ID {default_id} @ {baud})...", end=' ', flush=True) try: - client = create_config_client(motor_type, [], port, baud) - motors = client.scan_for_motors( - port=port, - id_range=(default_id, default_id), - baud_rates=[baud], - ) - if motors: + client = _make_client(motor_type, [], port, baud) + if client.scan_for_motors(port=port, id_range=(default_id, default_id), baud_rates=[baud]): print(f"{GREEN}found.{RESET}") return motor_type print("nothing.") @@ -137,520 +152,392 @@ def detect_motor_type(port: str) -> str | None: print(f"{YELLOW}error ({e}).{RESET}") return None -def play_success_beep(): - try: - subprocess.run(['paplay', '/usr/share/sounds/freedesktop/stereo/complete.oga'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) - except: - try: - subprocess.run(['beep', '-f', '800', '-l', '100'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) - except: - print('\a', end='', flush=True) - -def motor_color(model_name): - # Finger motors → blue; wrist motors → purple. Both motor families - # share the same role split, so the colour scheme matches. - if 'XC330' in model_name or 'HLS3915' in model_name: - return BLUE - if 'XC430' in model_name or 'HLS3930' in model_name: - return PURPLE - return '' def scan_for_default_motor(expected_type: str, port: str, motor_type: str, default_id: int, default_baud: int) -> bool: + """Return True if a factory-default motor of the expected model is present.""" try: - client = create_config_client(motor_type, [], port, default_baud) + client = _make_client(motor_type, [], port, default_baud) motors = client.scan_for_motors(port=port, id_range=(default_id, default_id), baud_rates=[default_baud]) if not motors: return False motor = motors[0] - expected_color = motor_color(expected_type) actual_color = motor_color(motor['model_name']) if expected_type in motor['model_name']: print(f"{GREEN}✓ Found default {actual_color}{motor['model_name']}{GREEN} motor{RESET}") return True - else: - print(f"{RED}❌ Wrong motor type: {actual_color}{motor['model_name']}{RESET} (expected {expected_color}{expected_type}{RESET}){RESET}") - return False + print( + f"{RED}❌ Wrong motor type: {actual_color}{motor['model_name']}{RESET}" + f" (expected {motor_color(expected_type)}{expected_type}{RESET})" + ) + return False except Exception as e: print(f"{RED}❌ Scan error: {e}{RESET}") return False + def configure_default_motor(target_id: int, port: str, target_baud: int, motor_type: str, default_id: int, default_baud: int) -> bool: + """Re-program a factory-default motor to ``target_id`` and ``target_baud``.""" try: - client = create_config_client(motor_type, [default_id], port, default_baud) - client.connect() - if not client.change_motor_id(default_id, target_id): - print(f"{RED}❌ Failed to change ID to {target_id}{RESET}") - _close_silently(client) - return False - _close_silently(client) + with _config_session(motor_type, [default_id], port, default_baud) as client: + if not client.change_motor_id(default_id, target_id): + logger.error("Failed to change ID to %d", target_id) + return False time.sleep(0.5) if target_baud != default_baud: - client = create_config_client(motor_type, [target_id], port, default_baud) - client.connect() - if not client.change_motor_baudrate(target_id, target_baud): - print(f"{RED}❌ Failed to change baud rate to {target_baud}{RESET}") - _close_silently(client) - return False - _close_silently(client) + with _config_session(motor_type, [target_id], port, default_baud) as client: + if not client.change_motor_baudrate(target_id, target_baud): + logger.error("Failed to change baud rate to %d", target_baud) + return False - print(f"{GREEN}✓ Successfully configured the new motor to ID={target_id}, baudrate={target_baud}{RESET}") + print(f"{GREEN}✓ Successfully configured motor → ID={target_id}, baudrate={target_baud}{RESET}") play_success_beep() return True except Exception as e: - print(f"{RED}❌ Error configuring motor: {e}{RESET}") + logger.error("Error configuring motor: %s", e) return False -def scan_already_configured_motors(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int, xc430_id: int = None) -> list: - print(f"\n🔍 Pre-Scanning for {YELLOW}already configured{RESET} motors...") + +def scan_already_configured_motors(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int, wrist_id: int | None = None) -> list: + """Pre-scan the chain for motors already at their target ID + baud. + + Returns the valid pre-configured motor IDs (contiguous, descending from + ``total_motors`` with optional wrist at ID 1). Aborts when motors are + present but don't form a valid prefix of the expected sequence — the + user must reset the offenders to factory defaults before re-running. + """ + print(f"\n🔍 Pre-scanning for {YELLOW}already configured{RESET} motors...") try: - client = create_config_client(motor_type, [], port, target_baud) + client = _make_client(motor_type, [], port, target_baud) motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) - # When target_baud == default_baud (Feetech), a factory-default motor - # sits at default_id with the finger model (HLS3915), so we drop it - # from the pre-scan to avoid mistaking it for a configured wrist. - # A genuine wrist (HLS3930 at ID 1) is kept — the model name disambiguates. + + finger_model = MOTOR_MODELS[motor_type]['finger'] + wrist_model = MOTOR_MODELS[motor_type]['wrist'] + + # On Feetech target_baud == default_baud, so a fresh HLS3915 at ID 1 + # is indistinguishable from a configured wrist; drop it (real HLS3930 + # wrist stays — model name disambiguates). if target_baud == default_baud: - motors = [ - m for m in motors - if m['id'] != default_id or 'HLS3930' in m['model_name'] - ] + motors = [m for m in motors if m['id'] != default_id or wrist_model in m['model_name']] if not motors: print(" No pre-configured motors detected") return [] + motor_by_id = {m['id']: m for m in motors} - valid_sequence, expected_id = [], total_motors - - if motor_type == 'feetech': - min_finger_id = 2 if xc430_id is not None else 1 - for mid in sorted(motor_by_id.keys(), reverse=True): - if mid in range(min_finger_id, total_motors + 1) and mid == expected_id: - if 'HLS3915' in motor_by_id[mid]['model_name']: - valid_sequence.append(mid) - expected_id -= 1 - else: - break - elif mid not in range(min_finger_id, total_motors + 1): - break - if xc430_id is not None and xc430_id in motor_by_id and expected_id == 1: - if 'HLS3930' in motor_by_id[xc430_id]['model_name']: - valid_sequence.append(xc430_id) - else: - min_xc330_id = 2 if xc430_id is not None else 1 - for mid in sorted(motor_by_id.keys(), reverse=True): - if mid in range(min_xc330_id, total_motors + 1) and mid == expected_id: - if 'XC330' in motor_by_id[mid]['model_name']: - valid_sequence.append(mid) - expected_id -= 1 - else: - break - elif mid not in range(min_xc330_id, total_motors + 1): - break - if xc430_id is not None and xc430_id in motor_by_id and expected_id == 1: - if 'XC430' in motor_by_id[xc430_id]['model_name']: - valid_sequence.append(xc430_id) - - invalid_motors = [mid for mid in motor_by_id.keys() if mid not in valid_sequence] + min_finger_id = 2 if wrist_id is not None else 1 + valid_sequence: list[int] = [] + expected_id = total_motors + + # Walk IDs descending; accept each only if it sits in the finger + # range, matches the next-expected ID, and is the right model. + # Stops at the first gap so partial chains never count as "valid". + for motor_id in sorted(motor_by_id.keys(), reverse=True): + if motor_id not in range(min_finger_id, total_motors + 1): + break + if motor_id != expected_id or finger_model not in motor_by_id[motor_id]['model_name']: + break + valid_sequence.append(motor_id) + expected_id -= 1 + + if wrist_id is not None and wrist_id in motor_by_id and expected_id == 1: + if wrist_model in motor_by_id[wrist_id]['model_name']: + valid_sequence.append(wrist_id) + + invalid_motors = [mid for mid in motor_by_id if mid not in valid_sequence] if valid_sequence: - print(f"{GREEN}\nFound {len(valid_sequence)} valid, pre-configured motors with correct ID order:{RESET}") - for mid in sorted(valid_sequence, reverse=True): - motor = motor_by_id[mid] - color = motor_color(motor['model_name']) - print(f" • ID {mid:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") + print(f"{GREEN}\nFound {len(valid_sequence)} valid, pre-configured motors:{RESET}") + for motor_id in sorted(valid_sequence, reverse=True): + _print_motor_row(motor_by_id[motor_id]) + if invalid_motors: print(f"{RED}Found {len(invalid_motors)} motors with invalid configuration:{RESET}") - for mid in sorted(invalid_motors, reverse=True): - motor = motor_by_id[mid] - color = motor_color(motor['model_name']) - print(f" • ID {mid:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps{RESET}") - print(f"{YELLOW}\nExpected sequence for {len(motor_by_id)} connected motors: {RESET}") + for motor_id in sorted(invalid_motors, reverse=True): + _print_motor_row(motor_by_id[motor_id]) + print(f"{YELLOW}\nExpected sequence for {len(motor_by_id)} connected motors:{RESET}") for i in range(total_motors, total_motors - len(motor_by_id), -1): - if xc430_id is not None and i == 1: - if motor_type == 'feetech': - print(f" • ID 1: {PURPLE}HLS3930{RESET} @ {target_baud:,} bps") - else: - print(f" • ID 1: {PURPLE}XC430-T240BB-T{RESET} @ {target_baud:,} bps") - else: - if motor_type == 'feetech': - print(f" • ID {i:2d}: {BLUE}HLS3915{RESET} @ {target_baud:,} bps") - else: - print(f" • ID {i:2d}: {BLUE}XC330-T288-T{RESET} @ {target_baud:,} bps") - defaults = MOTOR_TYPE_DEFAULTS[motor_type] - print(f"{RED}\n🚫 CONFIGURATION CANNOT CONTINUE{RESET}") - print(f" {RED}Please reset the relevant motors to factory default (ID={defaults['default_id']}, baud={defaults['default_baud']:,}) and restart.{RESET}") + model = wrist_model if (wrist_id is not None and i == 1) else finger_model + colour = PURPLE if model == wrist_model else BLUE + print(f" • ID {i:2d}: {colour}{model}{RESET} @ {target_baud:,} bps") + print( + f"{RED}\n🚫 CONFIGURATION CANNOT CONTINUE — reset the offending motors to " + f"factory default (ID={default_id}, baud={default_baud:,}) and restart.{RESET}" + ) sys.exit(1) return valid_sequence except Exception as e: print(f"{RED}❌ Error scanning: {e}{RESET}") return [] + def verify_all_motors(configured_ids: list, port: str, target_baud: int, total_motors: int, motor_type: str) -> bool: + """Re-scan after each configure step to detect dropped or duplicated motors.""" if not configured_ids: return True try: - client = create_config_client(motor_type, [], port, target_baud) - motors = client.scan_for_motors( - port=port, - id_range=(1, total_motors), - baud_rates=[target_baud] - ) - found_ids = sorted([m['id'] for m in motors]) + client = _make_client(motor_type, [], port, target_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) + found_ids = sorted(m['id'] for m in motors) expected_ids = sorted(configured_ids) if found_ids == expected_ids: return True - else: - print(f"{RED}❌ Motor verification failed!{RESET}") - print(f" Expected: {expected_ids}") - print(f" Found: {found_ids}") - print("\n🔍 Possible causes:") - print(" • Multiple motors with same default ID were connected (causes conflicts)") - print(" • Motor lost power or connection during configuration") - if motor_type == 'dynamixel': - print("Check wiring and/or use Dynamixel Wizard to inspect motor settings and connections.") - else: - print("Check wiring and inspect motor settings.") - return False + print(f"{RED}❌ Motor verification failed!{RESET}") + print(f" Expected: {expected_ids}") + print(f" Found: {found_ids}") + print(" Likely cause: duplicate default IDs on the bus, or a motor lost power.") + return False except Exception as e: print(f"{RED}❌ Error verifying motors: {e}{RESET}") return False -def get_valid_baudrates(motor_type: str) -> list: - """Get list of valid baudrates for the given motor type.""" - if motor_type == 'feetech': - return sorted(FEETECH_BAUD_RATE_MAP.keys(), reverse=True) - return sorted(DYNAMIXEL_BAUD_RATE_MAP.keys(), reverse=True) def change_motor_baudrate_only(motor_type: str, motor_id: int, port: str, current_baud: int, new_baud: int) -> bool: - """Change only the baudrate of a motor, leaving ID unchanged.""" + """Change a motor's baudrate, leaving its ID alone.""" + if current_baud == new_baud: + logger.info("Motor %d already at %s bps", motor_id, f"{new_baud:,}") + return True try: - if current_baud == new_baud: - print(f"{YELLOW} Motor {motor_id} already at {new_baud:,} bps{RESET}") - return True - - client = create_config_client(motor_type, [motor_id], port, current_baud) - client.connect() - if not client.change_motor_baudrate(motor_id, new_baud): - print(f"{RED}❌ Failed to change baud rate for motor {motor_id}{RESET}") - _close_silently(client) - return False - _close_silently(client) - - print(f"{GREEN}✓ Changed motor {motor_id} baudrate: {current_baud:,} → {new_baud:,}{RESET}") + with _config_session(motor_type, [motor_id], port, current_baud) as client: + if not client.change_motor_baudrate(motor_id, new_baud): + logger.error("Failed to change baud rate for motor %d", motor_id) + return False + print(f"{GREEN}✓ Motor {motor_id} baudrate: {current_baud:,} → {new_baud:,}{RESET}") return True except Exception as e: - print(f"{RED}❌ Error changing baudrate for motor {motor_id}: {e}{RESET}") + logger.error("Error changing baudrate for motor %d: %s", motor_id, e) return False + def reset_motor_to_factory(motor_type: str, motor_id: int, port: str, current_baud: int, default_id: int, default_baud: int) -> bool: + """Revert a configured motor back to factory defaults.""" try: if current_baud != default_baud: - client = create_config_client(motor_type, [motor_id], port, current_baud) - client.connect() - if not client.change_motor_baudrate(motor_id, default_baud): - print(f"{RED}❌ Failed to change baud rate for motor {motor_id}{RESET}") - _close_silently(client) - return False - _close_silently(client) + with _config_session(motor_type, [motor_id], port, current_baud) as client: + if not client.change_motor_baudrate(motor_id, default_baud): + logger.error("Failed to change baud rate for motor %d", motor_id) + return False time.sleep(0.5) - - client = create_config_client(motor_type, [motor_id], port, default_baud) - client.connect() - if not client.change_motor_id(motor_id, default_id): - print(f"{RED}❌ Failed to change ID for motor {motor_id}{RESET}") - _close_silently(client) - return False - _close_silently(client) - + with _config_session(motor_type, [motor_id], port, default_baud) as client: + if not client.change_motor_id(motor_id, default_id): + logger.error("Failed to change ID for motor %d", motor_id) + return False print(f"{GREEN}✓ Reset motor {motor_id} → ID={default_id}, baudrate={default_baud:,}{RESET}") return True except Exception as e: - print(f"{RED}❌ Error resetting motor {motor_id}: {e}{RESET}") + logger.error("Error resetting motor %d: %s", motor_id, e) return False -def baudrate_change_loop(port: str, current_baud: int, new_baud: int, total_motors: int, motor_type: str): - """Scan for motors and change their baudrate without modifying IDs.""" - print(f"\n{BOLD}🔄 BAUDRATE CHANGE MODE{RESET} — Changing all motors to {new_baud:,} bps...") - print(f" Press {BOLD}Ctrl+C{RESET} to stop.") - if motor_type == 'feetech': +# --- Bulk loops ------------------------------------------------------------- + +def _process_motors_loop(port: str, scan_baud: int, total_motors: int, motor_type: str, action, label: str, prompt: str): + """Generic scan-and-act loop used by --reset and --baudrate modes.""" + print(f"\n🔍 {label} — press {BOLD}Ctrl+C{RESET} to stop.") + if motor_type == FEETECH: while True: - feetech_safe_connect_prompt(port, "Connect the motor(s) you want to change baudrate") - print(f"\n🔍 Scanning for motors at {current_baud:,} bps...") - client = create_config_client(motor_type, [], port, current_baud) - motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[current_baud]) + feetech_safe_connect_prompt(port, prompt) + client = _make_client(motor_type, [], port, scan_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[scan_baud]) if not motors: - print(f"{YELLOW} No motors found at {current_baud:,} bps. Check connections.{RESET}") + print(f"{YELLOW} No motors found. Check connections.{RESET}") continue for motor in motors: - color = motor_color(motor['model_name']) - print(f" Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") - change_motor_baudrate_only( - motor_type, motor['id'], port, motor['baud_rate'], new_baud - ) + _print_motor_row(motor) + action(motor) else: - print(f"\n🔍 Waiting for motors to be connected at {current_baud:,} bps... (Ctrl+C to stop)") - known_ids = set() + known_ids: set[int] = set() while True: - client = create_config_client(motor_type, [], port, current_baud) - motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[current_baud]) + client = _make_client(motor_type, [], port, scan_baud) + motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[scan_baud]) found_ids = {m['id'] for m in motors} - new_motors = [m for m in motors if m['id'] not in known_ids] - - if new_motors: - for motor in new_motors: - color = motor_color(motor['model_name']) - print(f"\n Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") - change_motor_baudrate_only( - motor_type, motor['id'], port, motor['baud_rate'], new_baud - ) - print(f"\n🔍 Waiting for motors to be connected at {current_baud:,} bps... (Ctrl+C to stop)") - + for motor in [m for m in motors if m['id'] not in known_ids]: + _print_motor_row(motor) + action(motor) known_ids = found_ids time.sleep(1) -def reset_loop(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int): - print(f"\n{BOLD}🔄 RESET MODE{RESET} — Scanning for configured motors to reset to factory defaults...") - print(f" Factory defaults: ID={default_id}, baudrate={default_baud:,}") - print(f" Press {BOLD}Ctrl+C{RESET} to stop.") - if motor_type == 'feetech': - while True: - feetech_safe_connect_prompt(port, "Connect the motor(s) you want to reset") - print(f"\n🔍 Scanning for motors to reset...") - client = create_config_client(motor_type, [], port, target_baud) - motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) - if not motors: - print(f"{YELLOW} No motors found. Check connections.{RESET}") - continue - for motor in motors: - color = motor_color(motor['model_name']) - print(f" Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") - reset_motor_to_factory( - motor_type, motor['id'], port, motor['baud_rate'], - default_id, default_baud - ) - else: - print(f"\n🔍 Waiting for a configured motor to be connected... (Ctrl+C to stop)") - known_ids = set() - while True: - client = create_config_client(motor_type, [], port, target_baud) - motors = client.scan_for_motors(port=port, id_range=(1, total_motors), baud_rates=[target_baud]) - found_ids = {m['id'] for m in motors} - new_motors = [m for m in motors if m['id'] not in known_ids] - - if new_motors: - for motor in new_motors: - color = motor_color(motor['model_name']) - print(f"\n Found motor: ID {motor['id']:2d}: {color}{motor['model_name']}{RESET} @ {motor['baud_rate']:,} bps") - reset_motor_to_factory( - motor_type, motor['id'], port, motor['baud_rate'], - default_id, default_baud - ) - print(f"\n🔍 Waiting for a configured motor to be connected... (Ctrl+C to stop)") +def baudrate_change_loop(port: str, current_baud: int, new_baud: int, total_motors: int, motor_type: str): + _process_motors_loop( + port, current_baud, total_motors, motor_type, + action=lambda m: change_motor_baudrate_only(motor_type, m['id'], port, m['baud_rate'], new_baud), + label=f"Changing motor baudrates to {new_baud:,} bps", + prompt="Connect the motor(s) you want to change baudrate", + ) - known_ids = found_ids - time.sleep(1) -def main(): - logging.basicConfig(level=logging.WARNING, format='%(levelname)s: %(message)s') +def reset_loop(port: str, target_baud: int, total_motors: int, motor_type: str, default_id: int, default_baud: int): + _process_motors_loop( + port, target_baud, total_motors, motor_type, + action=lambda m: reset_motor_to_factory(motor_type, m['id'], port, m['baud_rate'], default_id, default_baud), + label=f"Resetting motors to defaults (ID={default_id}, baud={default_baud:,})", + prompt="Connect the motor(s) you want to reset", + ) - parser = argparse.ArgumentParser( - description="Configure motor chain for OrcaHand Assembly. Specify the path to the config file of your OrcaHand model.") - parser.add_argument( - "config_path", - type=str, - nargs="?", - default=None, - help="Path to config.yaml (e.g., orca_core/models/v2/orcahand_right/config.yaml). " - "Defaults to the bundled model when omitted.") - parser.add_argument( - "--reset", - action="store_true", - help="Reset configured motors back to factory defaults (runs in a loop until Ctrl+C)") - parser.add_argument( - "--baudrate", - type=int, - metavar="BAUD", - help="Change all motors to the specified baudrate without modifying IDs (runs in a loop until Ctrl+C)") - parser.add_argument( - "--motor-type", - type=str, - choices=list(MOTOR_TYPE_DEFAULTS.keys()), - help="Override motor family. Auto-detected from factory defaults " - "(Dynamixel @ 57600 / Feetech @ 1M, both ID 1) when omitted.") - args = parser.parse_args() +# --- main ------------------------------------------------------------------- + +def _load_config(config_path_arg: str | None) -> dict: + """Resolve the config path and read the yaml, exiting on error.""" try: - if args.config_path is None: + if config_path_arg is None: config_path = os.path.join(get_model_path(), "config.yaml") else: - config_path = os.path.abspath(args.config_path) + config_path = os.path.abspath(config_path_arg) if not os.path.exists(config_path): print(f"{RED}❌ config.yaml not found: {config_path}{RESET}") - return 1 - config = read_yaml(config_path) - PORT = config.get('port', '/dev/ttyUSB0') - motor_ids = config.get('motor_ids', list(range(17, 0, -1))) - TOTAL_MOTORS = len(motor_ids) + sys.exit(1) + return read_yaml(config_path) except Exception as e: print(f"{RED}❌ Error loading config: {e}{RESET}") - return 1 + sys.exit(1) - # Pick a port before probing — auto-detect needs an actual device path. - PORT = validate_or_detect_port(PORT, config_path, args.motor_type or 'dynamixel') - motor_type = args.motor_type +def _resolve_motor_type(explicit: str | None, port: str) -> str: + if explicit is not None: + return explicit + print(f"\n{BOLD}Auto-detecting motor family on {port}...{RESET}") + motor_type = detect_motor_type(port) if motor_type is None: - print(f"\n{BOLD}Auto-detecting motor family on {PORT}...{RESET}") - motor_type = detect_motor_type(PORT) - if motor_type is None: - print(f"\n{RED}❌ No motors responded at either family's factory defaults.{RESET}") - print(" - Check that one motor is connected and powered.") - print(" - Confirm motors are still at factory defaults (re-runs of this script change them).") - print(" - Or pass --motor-type {dynamixel,feetech} to skip auto-detection.") - return 1 - print(f"{GREEN}✓ Detected {motor_type} motors.{RESET}\n") + print( + f"\n{RED}❌ No motors responded at either family's factory defaults.{RESET}\n" + " Check power, wiring, and that motors are still at factory defaults\n" + " (re-runs of this script change them). Or pass --motor-type explicitly." + ) + sys.exit(1) + print(f"{GREEN}✓ Detected {motor_type} motors.{RESET}\n") + return motor_type - TARGET_BAUD = config.get('baudrate') or 1000000 - defaults = MOTOR_TYPE_DEFAULTS[motor_type] - default_id = defaults['default_id'] - default_baud = defaults['default_baud'] +def main(): + logging.basicConfig(level=logging.WARNING, format='%(levelname)s: %(message)s') - motor_type_label = 'Dynamixel' if motor_type == 'dynamixel' else 'Feetech' + parser = argparse.ArgumentParser( + description="Configure motor chain for OrcaHand Assembly. Specify the path to the config file of your OrcaHand model.") + parser.add_argument( + "config_path", type=str, nargs="?", default=None, + help="Path to config.yaml. Defaults to the bundled model when omitted.") + parser.add_argument( + "--reset", action="store_true", + help="Reset configured motors back to factory defaults (loops until Ctrl+C).") + parser.add_argument( + "--baudrate", type=int, metavar="BAUD", + help="Change all motors to the given baudrate without modifying IDs (loops until Ctrl+C).") + parser.add_argument( + "--motor-type", type=str, choices=list(MOTOR_TYPE_DEFAULTS.keys()), + help="Override motor family. Auto-detected from factory defaults when omitted.") + args = parser.parse_args() + + config = _load_config(args.config_path) + motor_ids = config.get('motor_ids', list(range(17, 0, -1))) + total_motors = len(motor_ids) + port = validate_or_detect_port(config.get('port', '/dev/ttyUSB0'), args.motor_type or DYNAMIXEL) + motor_type = _resolve_motor_type(args.motor_type, port) + + target_baud = config.get('baudrate') or 1_000_000 + defaults = MOTOR_TYPE_DEFAULTS[motor_type] + default_id, default_baud = defaults['default_id'], defaults['default_baud'] + label = motor_type.capitalize() if args.reset: - print("\n" + "=" * 60) - print(f"{BOLD}🔄 ORCAHAND {motor_type_label.upper()} CHAIN RESET 🔄{RESET}") - print("=" * 60) + _banner(f"🔄 ORCAHAND {label.upper()} CHAIN RESET 🔄") try: - reset_loop(PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type, default_id, default_baud) + reset_loop(port, target_baud, total_motors, motor_type, default_id, default_baud) except KeyboardInterrupt: print(f"\n\n{GREEN}Reset mode stopped.{RESET}\n") return 0 if args.baudrate is not None: - valid_baudrates = get_valid_baudrates(motor_type) + valid_baudrates = ( + sorted(FEETECH_BAUD_RATE_MAP.keys(), reverse=True) if motor_type == FEETECH + else sorted(DYNAMIXEL_BAUD_RATE_MAP.keys(), reverse=True) + ) if args.baudrate not in valid_baudrates: - print(f"{RED}❌ Invalid baudrate {args.baudrate:,} for {motor_type_label} motors{RESET}") - print(f"\n{YELLOW}Valid baudrates for {motor_type_label}:{RESET}") - for baud in valid_baudrates: - print(f" • {baud:,} bps") + print(f"{RED}❌ Invalid baudrate {args.baudrate:,} for {label} motors. " + f"Valid: {[f'{b:,}' for b in valid_baudrates]}{RESET}") return 1 - - print("\n" + "=" * 60) - print(f"{BOLD}🔄 ORCAHAND {motor_type_label.upper()} BAUDRATE CHANGE 🔄{RESET}") - print("=" * 60) - print(f"\nChanging all motors to baudrate: {BOLD}{args.baudrate:,} bps{RESET}") - print(f"Current baudrate in config: {BOLD}{TARGET_BAUD:,} bps{RESET}") - print(f"\n{YELLOW}Note: Motor IDs will NOT be changed, only baudrate.{RESET}") + _banner(f"🔄 ORCAHAND {label.upper()} BAUDRATE CHANGE 🔄") + print(f"\nChanging all motors to {BOLD}{args.baudrate:,} bps{RESET} " + f"(current config baud: {target_baud:,}). IDs unchanged.") try: - baudrate_change_loop(PORT, TARGET_BAUD, args.baudrate, TOTAL_MOTORS, motor_type) + baudrate_change_loop(port, target_baud, args.baudrate, total_motors, motor_type) except KeyboardInterrupt: print(f"\n\n{GREEN}Baudrate change mode stopped.{RESET}\n") return 0 - print("\n" + "=" * 60) - print(f"{BOLD}⚙️ ORCAHAND {motor_type_label.upper()} CHAIN CONFIGURATION ⚙️{RESET}") - print() + # Full chain configuration. + _banner(f"⚙️ ORCAHAND {label.upper()} CHAIN CONFIGURATION ⚙️") has_wrist = 'wrist' in config.get('joint_to_motor_map', {}) - if has_wrist: - WRIST_ID = 1 - FINGER_IDS = sorted([mid for mid in motor_ids if mid != 1], reverse=True) - else: - WRIST_ID = None - FINGER_IDS = sorted(motor_ids, reverse=True) - - if motor_type == 'dynamixel': - finger_model, wrist_model = 'XC330-T288-T', 'XC430-T240BB-T' - else: - finger_model, wrist_model = 'HLS3915', 'HLS3930' - - print(f"\nFor the chosen hand model, we need to configure {TOTAL_MOTORS} motors in total:") - print(f" • {len(FINGER_IDS)} {BLUE}{finger_model}{RESET} motors: {BOLD}IDs {min(FINGER_IDS)}-{max(FINGER_IDS)}{RESET} @ {TARGET_BAUD:,} bps") - if WRIST_ID is not None: - print(f" • 1 {PURPLE}{wrist_model}{RESET} motor: {BOLD}ID 1{RESET} @ {TARGET_BAUD:,} bps") + wrist_id = 1 if has_wrist else None + finger_ids = sorted( + [mid for mid in motor_ids if mid != 1] if has_wrist else motor_ids, + reverse=True, + ) + finger_model = MOTOR_MODELS[motor_type]['finger'] + wrist_model = MOTOR_MODELS[motor_type]['wrist'] + + print(f"\nConfiguring {total_motors} motors:") + print(f" • {len(finger_ids)} {BLUE}{finger_model}{RESET}: {BOLD}IDs {min(finger_ids)}-{max(finger_ids)}{RESET} @ {target_baud:,} bps") + if wrist_id is not None: + print(f" • 1 {PURPLE}{wrist_model}{RESET}: {BOLD}ID 1{RESET} @ {target_baud:,} bps") print("=" * 60) - all_target_ids = FINGER_IDS + ([WRIST_ID] if WRIST_ID is not None else []) - XC430_ID = WRIST_ID # back-compat: scan helpers still use the old name - - already_configured = scan_already_configured_motors(PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type, default_id, default_baud, XC430_ID).copy() - configured_ids = already_configured.copy() - remaining_ids = [id for id in all_target_ids if id not in already_configured] - - if remaining_ids: - print(f"\n⏳ {BOLD}{len(remaining_ids)} motors{RESET} with IDs: {remaining_ids} {YELLOW}remaining{RESET}.") - print("\n" + "─" * 60) - print(f"\n🔧 {BOLD}Troubleshooting guide:{RESET}") - print(" ✓ Board connection: Properly connected to power and computer") - print(" ✓ Motor connection: Properly wired to daisy chain") - print(f" ✓ Motor settings: ID={default_id}, baudrate={default_baud:,} (factory default)") - print(f" ✓ Serial port permissions (Linux): Your user must have read/write access to the serial port.") - print(f" Run {BOLD}sudo usermod -aG dialout $USER{RESET} and re-login, or {BOLD}sudo chmod 666 /dev/ttyUSB0{RESET} for a quick fix.") - print(f" ✓ Serial port path: Ensure {BOLD}config.yaml{RESET} has the correct port for your OS") - print(f" (e.g., {BOLD}/dev/ttyUSB0{RESET} on Linux, {BOLD}/dev/tty.usbserial-*{RESET} on macOS)") - print(f" Otherwise the motors {UNDERLINE}won't be detected{RESET} during scanning.") - print("\n" + "─" * 60) - else: - print("\n" + "=" * 60) - print("\nAll motors are already configured and verified!") - print(f"🚀 {ORANGE}{BOLD}Ready for operation!{RESET}") - print() + all_target_ids = finger_ids + ([wrist_id] if wrist_id is not None else []) + already_configured = scan_already_configured_motors( + port, target_baud, total_motors, motor_type, default_id, default_baud, wrist_id, + ) + configured_ids = list(already_configured) + remaining_ids = [mid for mid in all_target_ids if mid not in already_configured] + + if not remaining_ids: + print(f"\nAll motors already configured. 🚀 {ORANGE}{BOLD}Ready for operation!{RESET}\n") return 0 + print( + f"\n⏳ {BOLD}{len(remaining_ids)} motors{RESET} remaining: {remaining_ids}\n" + f" Factory defaults expected: ID={default_id}, baud={default_baud:,}.\n" + f" If a motor isn't detected, check power, daisy-chain wiring, and serial-port " + f"permissions ({BOLD}sudo usermod -aG dialout $USER{RESET} on Linux)." + ) + for step, target_id in enumerate(remaining_ids, len(already_configured) + 1): - is_wrist = (WRIST_ID is not None and target_id == 1) + is_wrist = (wrist_id is not None and target_id == 1) expected_model = wrist_model if is_wrist else finger_model color = motor_color(expected_model) + location = ( + f"the {BOLD}board{RESET}" if target_id == max(all_target_ids) + else f"the previous motor {BOLD}(ID {target_id + 1}){RESET}" + ) + msg = f"Connect a {color}{expected_model} {BOLD}(ID {target_id}){RESET}{ORANGE} to {location}" - if target_id == max(all_target_ids): - connection_msg = f"Connect a {color}{expected_model} {BOLD}(ID {target_id}){RESET}{ORANGE} to the {BOLD}board{RESET}" - else: - prev_id = target_id + 1 - connection_msg = f"Connect a {color}{expected_model} {BOLD}(ID {target_id}){RESET}{ORANGE} to the previous motor {BOLD}(ID {prev_id}){RESET}" - - print(f"{UNDERLINE}{ORANGE}\nSTEP {step}/{TOTAL_MOTORS}{RESET}") - - if motor_type == 'feetech': - feetech_safe_connect_prompt(PORT, connection_msg) + print(f"\n{BOLD}{ORANGE}STEP {step}/{total_motors}{RESET}") + if motor_type == FEETECH: + feetech_safe_connect_prompt(port, msg) else: - print(f"{connection_msg}") + print(msg) print(f"\n🔍 Scanning for default {color}{expected_model}{RESET} motor...") + while True: + if scan_for_default_motor(expected_model, port, motor_type, default_id, default_baud): + if not configure_default_motor(target_id, port, target_baud, motor_type, default_id, default_baud): + return 1 + configured_ids.append(target_id) + if not verify_all_motors(configured_ids, port, target_baud, total_motors, motor_type): + return 1 + break + time.sleep(1) - try: - while True: - if scan_for_default_motor(expected_model, PORT, motor_type, default_id, default_baud): - if configure_default_motor(target_id, PORT, TARGET_BAUD, motor_type, default_id, default_baud): - configured_ids.append(target_id) - if not verify_all_motors(configured_ids, PORT, TARGET_BAUD, TOTAL_MOTORS, motor_type): - return 1 - break - else: - return 1 - else: - time.sleep(1) - except Exception as e: - print(f"\n ERROR: {e}") - sys.exit(1) - - print("\n" + "=" * 60) - print("\nAll motors have been configured and verified!") - print(f"🚀 {ORANGE}{BOLD}Ready for operation!{RESET}") - print() + print(f"\nAll motors configured. 🚀 {ORANGE}{BOLD}Ready for operation!{RESET}\n") return 0 + if __name__ == '__main__': try: sys.exit(main()) except KeyboardInterrupt: - print("\n\n" + "─" * 60) - print(f"{RED}❌ CONFIGURATION INTERRUPTED ❌{RESET}") - print(f"\n ⚠️ {YELLOW}Not all motors have been fully configured yet!{RESET}") - print(" Rerun this script to continue with configuration.\n") + print(f"\n\n{RED}❌ CONFIGURATION INTERRUPTED{RESET}") + print(f" {YELLOW}Not all motors have been configured yet — rerun to continue.{RESET}\n") sys.exit(1) except Exception as e: print(f"\n ERROR: {e}") diff --git a/scripts/debug_overload.py b/scripts/debug_overload.py index 3ef45f83..2389ba2d 100644 --- a/scripts/debug_overload.py +++ b/scripts/debug_overload.py @@ -60,7 +60,7 @@ def main(): print("Operating mode set, torque re-enabled.") print(f"\n=== Step 4: Read position ===") - pos, vel, cur = dxl_client.read_pos_vel_cur() + pos, vel, cur = dxl_client.read_position_velocity_current() print(f"pos={pos[0]:.3f} vel={vel[0]:.3f} cur={cur[0]:.1f}") print("\nReboot and recovery successful!") diff --git a/scripts/test_overload.py b/scripts/test_overload.py index 0d618692..f945a700 100644 --- a/scripts/test_overload.py +++ b/scripts/test_overload.py @@ -37,7 +37,7 @@ def main(): try: while True: hw_err = dxl_client.read_hardware_error(mid) - pos, vel, cur = dxl_client.read_pos_vel_cur() + pos, vel, cur = dxl_client.read_position_velocity_current() target = pos + 0.3 dxl_client.write_desired_pos([mid], target) err_str = f" HW_ERR=0x{hw_err:02X}" if hw_err else "" diff --git a/tests/conftest.py b/tests/conftest.py index a45e8eb3..f8194a14 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -2,6 +2,7 @@ import sys import types from pathlib import Path +from types import SimpleNamespace import pytest @@ -47,6 +48,7 @@ def getData(self, motor_id: int, address: int, size: int) -> int: module.PortHandler = PortHandler module.PacketHandler = PacketHandler module.GroupBulkRead = GroupBulkRead + module.COMM_SUCCESS = 0 return module @@ -88,3 +90,23 @@ def connected_mock_hand(mock_config_dir: Path) -> MockOrcaHand: def initialized_mock_hand(connected_mock_hand: MockOrcaHand) -> MockOrcaHand: connected_mock_hand.init_joints(force_calibrate=True) return connected_mock_hand + + +@pytest.fixture +def patch_comports(monkeypatch): + """Replace serial.tools.list_ports.comports() with a fixed port list.""" + def _set(ports): + import serial.tools.list_ports as ltp + monkeypatch.setattr(ltp, "comports", lambda: ports) + return _set + + +@pytest.fixture +def mock_hand(mock_config_dir): + """Bare MockOrcaHand (not connected) for unit-testing helper methods.""" + return MockOrcaHand(config_path=str(mock_config_dir / "config.yaml")) + + +def fake_serial_port(device: str, vid: int) -> SimpleNamespace: + """Build a fake serial.tools.list_ports.ListPortInfo-like object.""" + return SimpleNamespace(device=device, vid=vid, description="fake") diff --git a/tests/test_connect_resolution.py b/tests/test_connect_resolution.py index 95d3c238..88f4f84b 100644 --- a/tests/test_connect_resolution.py +++ b/tests/test_connect_resolution.py @@ -1,56 +1,45 @@ """Tests for OrcaHand.connect() driver auto-detection.""" import dataclasses -from types import SimpleNamespace -import pytest +from types import SimpleNamespace from orca_core.constants import KNOWN_VIDS -from orca_core.hardware_hand import MockOrcaHand +from orca_core.hardware_hand import OrcaHand from orca_core.utils.utils import ( find_single_usb_serial_port, motor_type_for_port, ) +from .conftest import fake_serial_port + DYNAMIXEL_VID = KNOWN_VIDS["dynamixel"][0] FEETECH_VID = KNOWN_VIDS["feetech"][0] -def _fake_port(device: str, vid: int) -> SimpleNamespace: - return SimpleNamespace(device=device, vid=vid, description="fake") - - -@pytest.fixture -def patch_comports(monkeypatch): - def _set(ports): - import serial.tools.list_ports as ltp - monkeypatch.setattr(ltp, "comports", lambda: ports) - return _set - - # ----- USB VID lookup helpers --------------------------------------------- def test_motor_type_for_port_matches_known_vid(patch_comports): - patch_comports([_fake_port("/dev/cu.feetech", FEETECH_VID)]) + patch_comports([fake_serial_port("/dev/cu.feetech", FEETECH_VID)]) assert motor_type_for_port("/dev/cu.feetech") == "feetech" def test_motor_type_for_port_returns_none_for_unknown_vid(patch_comports): - patch_comports([_fake_port("/dev/cu.weird", 0xDEAD)]) + patch_comports([fake_serial_port("/dev/cu.weird", 0xDEAD)]) assert motor_type_for_port("/dev/cu.weird") is None def test_find_single_usb_returns_lone_adapter(patch_comports): - patch_comports([_fake_port("/dev/cu.weird", 0x2F5D)]) + patch_comports([fake_serial_port("/dev/cu.weird", 0x2F5D)]) assert find_single_usb_serial_port() == "/dev/cu.weird" def test_find_single_usb_returns_none_when_multiple(patch_comports): patch_comports( [ - _fake_port("/dev/cu.a", 0x2F5D), - _fake_port("/dev/cu.b", 0x2F5D), + fake_serial_port("/dev/cu.a", 0x2F5D), + fake_serial_port("/dev/cu.b", 0x2F5D), ] ) assert find_single_usb_serial_port() is None @@ -60,18 +49,15 @@ def test_find_single_usb_skips_non_usb_ports(patch_comports): patch_comports( [ SimpleNamespace(device="/dev/cu.bluetooth", vid=None, description=""), - _fake_port("/dev/cu.usb", 0x2F5D), + fake_serial_port("/dev/cu.usb", 0x2F5D), ] ) assert find_single_usb_serial_port() == "/dev/cu.usb" # ----- _trial_probe ------------------------------------------------------- - -@pytest.fixture -def mock_hand(mock_config_dir): - return MockOrcaHand(config_path=str(mock_config_dir / "config.yaml")) - +# Use the unbound OrcaHand._trial_probe so we exercise the real implementation +# (not MockOrcaHand's synthetic override). def test_trial_probe_finds_feetech(mock_hand, monkeypatch): from orca_core.hardware import dynamixel_client, feetech_client @@ -83,7 +69,7 @@ def test_trial_probe_finds_feetech(mock_hand, monkeypatch): "probe", staticmethod(lambda port, baudrate, motor_ids, **k: True), ) - motor_type, baudrate = OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + motor_type, baudrate = OrcaHand._trial_probe(mock_hand, "/dev/cu.x") assert motor_type == "feetech" assert baudrate == 1_000_000 @@ -101,7 +87,7 @@ def fake_probe(port, baudrate, motor_ids, **k): monkeypatch.setattr( feetech_client.FeetechClient, "probe", staticmethod(lambda *a, **k: False) ) - motor_type, baudrate = OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + motor_type, baudrate = OrcaHand._trial_probe(mock_hand, "/dev/cu.x") assert motor_type == "dynamixel" assert baudrate == 3_000_000 assert seen == [1_000_000, 3_000_000] # priority order @@ -115,30 +101,24 @@ def test_trial_probe_returns_none_when_nothing_responds(mock_hand, monkeypatch): monkeypatch.setattr( feetech_client.FeetechClient, "probe", staticmethod(lambda *a, **k: False) ) - assert OrcaHand_trial_probe(mock_hand, "/dev/cu.x") == (None, None) + assert OrcaHand._trial_probe(mock_hand, "/dev/cu.x") == (None, None) def test_trial_probe_honours_pinned_motor_type(mock_hand, monkeypatch): """When motor_type is pinned in yaml, only baudrates iterate.""" mock_hand.config = dataclasses.replace(mock_hand.config, motor_type="dynamixel") seen_types = set() - def fake_probe(port, baudrate, motor_ids, **k): - return False # never responds — we just want to see the iteration - from orca_core.hardware import dynamixel_client, feetech_client - def feetech_probe(*a, **k): + def fake_dxl_probe(port, baudrate, motor_ids, **k): + return False + def fake_feetech_probe(*a, **k): seen_types.add("feetech") return False + from orca_core.hardware import dynamixel_client, feetech_client monkeypatch.setattr( - dynamixel_client.DynamixelClient, "probe", staticmethod(fake_probe) + dynamixel_client.DynamixelClient, "probe", staticmethod(fake_dxl_probe) ) monkeypatch.setattr( - feetech_client.FeetechClient, "probe", staticmethod(feetech_probe) + feetech_client.FeetechClient, "probe", staticmethod(fake_feetech_probe) ) - OrcaHand_trial_probe(mock_hand, "/dev/cu.x") + OrcaHand._trial_probe(mock_hand, "/dev/cu.x") assert "feetech" not in seen_types - - -def OrcaHand_trial_probe(hand, port): - """Hop over the mock-class override to test the real implementation.""" - from orca_core.hardware_hand import OrcaHand - return OrcaHand._trial_probe(hand, port) From 78c3eb3d4c3733890aae1caf087de857d3a9a9d1 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 12 May 2026 11:16:07 +0200 Subject: [PATCH 07/10] Probe first/last motor ID and hoist client imports probe() now pings motor_ids[0] and motor_ids[-1] instead of the first two, so partial bench setups (e.g. only the last motor on the chain) get detected. Drops the unused ping_count parameter. Promotes DynamixelClient/FeetechClient imports in hardware_hand to module level to avoid per-call name resolution in _trial_probe and _create_motor_client. --- orca_core/hardware/dynamixel_client.py | 12 ++++++++---- orca_core/hardware/feetech_client.py | 12 ++++++++---- orca_core/hardware_hand.py | 15 +++------------ 3 files changed, 19 insertions(+), 20 deletions(-) diff --git a/orca_core/hardware/dynamixel_client.py b/orca_core/hardware/dynamixel_client.py index deccb5e7..dfe7fb7c 100644 --- a/orca_core/hardware/dynamixel_client.py +++ b/orca_core/hardware/dynamixel_client.py @@ -225,14 +225,18 @@ def probe( port: str, baudrate: int, motor_ids: Sequence[int], - ping_count: int = 2, ) -> bool: - """Open ``port`` at ``baudrate`` and ping the first few motor IDs. + """Open ``port`` at ``baudrate`` and ping the first and last motor IDs. - Returns True if any motor responds — i.e. the bus is speaking the + Returns True if either motor responds — i.e. the bus is speaking the Dynamixel Protocol 2.0 at this baudrate. Used at connect time to auto-detect the driver family without enabling torque. """ + ids = list(motor_ids) + if not ids: + return False + sample = [ids[0]] if len(ids) == 1 else [ids[0], ids[-1]] + handler = PortHandler(port) try: if not handler.openPort(): @@ -240,7 +244,7 @@ def probe( if not handler.setBaudRate(baudrate): return False packet = PacketHandler(PROTOCOL_VERSION) - for motor_id in list(motor_ids)[:ping_count]: + for motor_id in sample: _, comm, _ = packet.ping(handler, motor_id) if comm == COMM_SUCCESS: return True diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index 13799e9f..1c311dbc 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -632,21 +632,25 @@ def probe( port: str, baudrate: int, motor_ids: Sequence[int], - ping_count: int = 2, ) -> bool: - """Open ``port`` at ``baudrate`` and ping the first few motor IDs. + """Open ``port`` at ``baudrate`` and ping the first and last motor IDs. - Returns True if any motor responds — i.e. the bus is speaking the + Returns True if either motor responds — i.e. the bus is speaking the Feetech protocol at this baudrate. Used at connect time to auto-detect the driver family without enabling torque. """ + ids = list(motor_ids) + if not ids: + return False + sample = [ids[0]] if len(ids) == 1 else [ids[0], ids[-1]] + handler = PortHandler(port) handler.baudrate = baudrate try: if not handler.openPort(): return False packet = sms_sts(handler) - for motor_id in list(motor_ids)[:ping_count]: + for motor_id in sample: _, comm, _ = packet.ping(motor_id) if comm == COMM_SUCCESS: return True diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 971e2bda..11363190 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -13,13 +13,15 @@ import time from collections import deque from threading import RLock -from typing import Dict, List, TYPE_CHECKING, Union +from typing import Dict, List, Union import numpy as np from .base_hand import BaseHand from .calibration import CalibrationResult from .hand_config import OrcaHandConfig +from .hardware.dynamixel_client import DynamixelClient +from .hardware.feetech_client import FeetechClient from .hardware.motor_client import MotorClient from .utils.utils import ( auto_detect_port, @@ -29,10 +31,6 @@ update_yaml, ) -if TYPE_CHECKING: - from .hardware.dynamixel_client import DynamixelClient - from .hardware.feetech_client import FeetechClient - from .constants import ( SUPPORTED_MOTOR_TYPES, MOTOR_BAUD_RATES, @@ -142,13 +140,9 @@ def _create_motor_client( baudrate: int, ) -> MotorClient: if motor_type == "dynamixel": - from .hardware.dynamixel_client import DynamixelClient - return DynamixelClient(self.config.motor_ids, port, baudrate) if motor_type == "feetech": - from .hardware.feetech_client import FeetechClient - return FeetechClient(self.config.motor_ids, port, baudrate) raise ValueError( @@ -181,9 +175,6 @@ def _trial_probe(self, port: str) -> tuple[str | None, int | None]: ``baudrate`` is pinned in yaml, that dimension is fixed and the probe only iterates the other. """ - from .hardware.dynamixel_client import DynamixelClient - from .hardware.feetech_client import FeetechClient - candidates = { "dynamixel": DynamixelClient, "feetech": FeetechClient, From dbfa4fe36a156208702d00a8d5ed9c41900eb221 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 12 May 2026 11:35:18 +0200 Subject: [PATCH 08/10] Document optional driver overrides in README and setting-up-config MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit port / baudrate / motor_type were removed from bundled configs when auto-detection landed, but the override path (drop them back into config.yaml when you need to) wasn't documented anywhere users would find it. Now covered in: - README "Serial port, baudrate, and motor type" section — replaces the outdated "Serial Port Path" block that told users to set the port in yaml. New section explains auto-detect + three override use cases (multiple adapters / non-default baud / explicit family). - setting-up-config.md General Settings — bundled yaml example trimmed, new "Optional driver overrides" subsection mirrors the README. --- README.md | 28 +++++++++---------- .../getting-started-docs/setting-up-config.md | 22 +++++++++++++-- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index 40a2bba8..3d669cdb 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ To get started with Orca Core, follow these steps: ### Serial Port Permissions (Linux) -On Linux, the serial port (e.g., `/dev/ttyUSB0`) is owned by the `dialout` group. If your user is not in this group, you will get a **permission denied** error and motors won't be detected. +On Linux, the serial port (e.g., `/dev/ttyACM0`) is owned by the `dialout` group. If your user is not in this group, you will get a **permission denied** error and motors won't be detected. **Permanent fix** (requires re-login): @@ -76,23 +76,23 @@ sudo usermod -aG dialout $USER **Temporary fix** (resets on reboot/replug): ```sh -sudo chmod 666 /dev/ttyUSB0 +sudo chmod 666 /dev/ttyACM0 ``` -### Serial Port Path +### Serial port, baudrate, and motor type -Make sure the `port` field in your `config.yaml` matches your operating system: +By default these are all **auto-detected** at connect time. -| OS | Example port | -|-------|---------------------------------| -| Linux | `/dev/ttyUSB0` | -| macOS | `/dev/tty.usbserial-XXXXXXXX` | +However, you can declare them explicitly in `config.yaml`. Useful when: ---- +- **multiple hands are connected** at once → `port` disambiguates which one +- **motors run at a non-default baudrate** → `baudrate` skips the probe sweep +- **the auto-detection picks the wrong family** → `motor_type` forces a specific one -**Note:** -- Always ensure your `config.yaml` matches your hardware and wiring. -- All scripts in the `scripts/` folder take the model path as their first argument. -- For more advanced usage, see the other scripts and the API documentation. +```yaml +# Optional overrides: auto-detected if omitted +port: /dev/ttyACM0 # or /'dev/cu.usbmodemXXXX' on macOS +baudrate: 1000000 # 1M for v2; 3M for v1 +motor_type: dynamixel # or 'feetech' +``` ---- diff --git a/docs/pages/getting-started-docs/setting-up-config.md b/docs/pages/getting-started-docs/setting-up-config.md index 56337b13..cb97d9eb 100644 --- a/docs/pages/getting-started-docs/setting-up-config.md +++ b/docs/pages/getting-started-docs/setting-up-config.md @@ -15,8 +15,6 @@ This file defines parameters crucial for the hand's operation, including communi ```yaml version: 0.2.0 -baudrate: 3000000 -port: /dev/ttyUSB0 max_current: 400 type: right control_mode: current_based_position @@ -24,7 +22,25 @@ control_mode: current_based_position **What should be changed?** -You should change the `port` to match your system (Linux or macOS). Change `type` to right or left depending on the hand assembly you have. `max_current` is set to a value found to be sufficient; you can adjust it depending on the needs of your tasks. The `baudrate` or `control_mode` should not be changed based on the current implementation in the repo. If you decide to change them you have to adapt the code accordingly. +Change `type` to right or left depending on the hand assembly. `max_current` is set to a sane default; adjust if your tasks need more or less. `control_mode` should generally stay at `current_based_position`. + +#### Optional driver overrides + +`port`, `baudrate`, and `motor_type` are **not in the bundled configs** — they're auto-detected at connect time: + +- The serial port is found by USB VID, falling back to "the only adapter present" or an interactive picker. +- The motor family (Dynamixel vs Feetech) is identified by pinging factory defaults on the bus. +- The baudrate comes from the family's known set (1M / 3M for Dynamixel, 1M for Feetech). + +Drop any of these into `config.yaml` if you need to override that behaviour: + +```yaml +port: /dev/cu.usbmodemXXXX # only when multiple adapters are connected +baudrate: 1000000 # only when motors are configured for a non-default rate +motor_type: feetech # only when probing might misidentify the bus +``` + +You won't normally need any of them — the script will tell you on the command line if a probe failed and asking for an override would help. --- From a4cddf435b89afc8278ed58171f94f5c1cc0f802 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 12 May 2026 12:06:58 +0200 Subject: [PATCH 09/10] Persist auto-detected motor_type / baudrate to config.yaml on first probe MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When motor_type or baudrate is missing from yaml the connect-time probe fills it in, and the result is now written back to the file. Subsequent connects find the populated values and skip the probe entirely. Each field is only persisted if the yaml didn't have it — manual edits stick. To swap motor families on the same hand, clear the field(s) in yaml and reconnect to trigger a fresh probe. _persist_resolved_port becomes _persist_resolved_driver and handles all three fields uniformly; a single console line announces which were written so the yaml mutation isn't silent. --- orca_core/hardware_hand.py | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 11363190..3cc5e579 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -239,19 +239,37 @@ def connect(self) -> tuple[bool, str]: with self._motor_lock: self._motor_client.connect() - self._persist_resolved_port(port) + self._persist_resolved_driver(motor_type, port, baudrate) return True, f"Connection successful ({motor_type} @ {port}, {baudrate} baud)" except Exception as e: self._motor_client = None return False, f"Connection failed on {port}: {str(e)}" - def _persist_resolved_port(self, port: str) -> None: - """Write the resolved port back to config.yaml when it differs.""" - if self.config.port == port: + def _persist_resolved_driver(self, motor_type: str, port: str, baudrate: int) -> None: + """Persist auto-detected driver fields to config.yaml. + + Each field is only written when it was missing from yaml. Once written, + the next connect short-circuits the probe and uses the yaml values + directly. Users who want to swap motor families on the same hand can + clear the yaml fields and re-run to trigger a fresh probe. + """ + updates = {} + if not self.config.port or self.config.port != port: + updates["port"] = port + if self.config.motor_type is None: + updates["motor_type"] = motor_type + if self.config.baudrate is None: + updates["baudrate"] = baudrate + if not updates: return - self.config = dataclasses.replace(self.config, port=port) - update_yaml(self.config.config_path, "port", port) + self.config = dataclasses.replace(self.config, **updates) + for key, value in updates.items(): + update_yaml(self.config.config_path, key, value) + print( + f"Wrote auto-detected {', '.join(updates.keys())} to " + f"{os.path.basename(self.config.config_path)}." + ) def disconnect(self) -> tuple[bool, str]: """Disable torque and close the serial connection. From 5e5323523bdea6ade18d4b7ed9dbf4e1e25c9521 Mon Sep 17 00:00:00 2001 From: Maximilian Eberlein Date: Tue, 12 May 2026 12:25:10 +0200 Subject: [PATCH 10/10] Drop unsafe defaults on scan_for_motors signatures MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `port: str = '/dev/ttyUSB0'` and `id_range: tuple = (0, 252)` were vestigial — every caller passes both arguments explicitly. The Linux- path default was actively misleading on macOS / custom adapters, and the (0, 252) full-address-space default would silently make any unguarded call do a 253-ID scan. Made both required on FeetechClient and DynamixelClient; baud_rates stays Optional[list] = None since that fallback is legitimately useful. Also switched DynamixelClient.scan_for_motors's baud_rates fallback from `... or list(...)` to an explicit `if baud_rates is None`, matching the FeetechClient pattern. Extends Francesco's "I would not add this default, rather use a None" review note (PR #64) to the two other defaults in the same signature. --- orca_core/hardware/dynamixel_client.py | 5 +++-- orca_core/hardware/feetech_client.py | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/orca_core/hardware/dynamixel_client.py b/orca_core/hardware/dynamixel_client.py index dfe7fb7c..290392be 100644 --- a/orca_core/hardware/dynamixel_client.py +++ b/orca_core/hardware/dynamixel_client.py @@ -563,10 +563,11 @@ def change_motor_baudrate(self, motor_id: int, new_baud_rate: int) -> bool: logging.error(f"Failed to change baud rate: {e}") return False - def scan_for_motors(self, port: str = '/dev/ttyUSB0', id_range: tuple = (0, 252), + def scan_for_motors(self, port: str, id_range: tuple, baud_rates: Optional[list] = None) -> list: """Scans for Dynamixel motors. Returns list of {'id', 'baud_rate', 'model_number', 'model_name'}.""" - baud_rates = baud_rates or list(BAUD_RATE_MAP.keys()) + if baud_rates is None: + baud_rates = list(BAUD_RATE_MAP.keys()) detected_motors = [] for baud_rate in baud_rates: port_handler = self.dxl.PortHandler(port) diff --git a/orca_core/hardware/feetech_client.py b/orca_core/hardware/feetech_client.py index 1c311dbc..fa4cec4b 100644 --- a/orca_core/hardware/feetech_client.py +++ b/orca_core/hardware/feetech_client.py @@ -196,8 +196,8 @@ def disconnect(self) -> None: @staticmethod def scan_for_motors( - port: str = '/dev/ttyUSB0', - id_range: tuple = (0, 252), + port: str, + id_range: tuple, baud_rates: Optional[list] = None, ) -> list: """Scan ``port`` for any responding Feetech motors.