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. --- 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/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..290392be 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 @@ -214,6 +220,41 @@ 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], + ) -> bool: + """Open ``port`` at ``baudrate`` and ping the first and last motor IDs. + + 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(): + return False + if not handler.setBaudRate(baudrate): + return False + packet = PacketHandler(PROTOCOL_VERSION) + for motor_id in sample: + _, 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: @@ -273,13 +314,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""" @@ -521,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) @@ -635,7 +678,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: @@ -644,8 +687,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. @@ -653,28 +700,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: + """Hook overridden by subclasses to read each motor individually. Default no-op keeps cached values.""" + def _initialize_data(self): """Initializes the cached data.""" self._data = np.zeros(len(self.motor_ids), dtype=np.float32) @@ -729,6 +782,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(), @@ -746,6 +843,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() @@ -782,7 +902,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 319b30df..fa4cec4b 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, MotorRead from .feetech import ( PortHandler, sms_sts, @@ -27,12 +27,34 @@ SMS_STS_PRESENT_SPEED_L, SMS_STS_PRESENT_CURRENT_L, SMS_STS_PRESENT_TEMPERATURE, + SMS_STS_MOVING, SMS_STS_ACC, 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. Multiple raw values can map to the same label. +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 @@ -42,6 +64,12 @@ POS_MIN = 0 POS_MAX = 4095 +# Feetech servos rotate in the opposite direction from Dynamixel for the same +# raw command. We invert here so OrcaHand sees a single, motor-type-agnostic +# convention; per-joint sign tuning (joint_to_motor_map) can then be shared +# across motor types. +POSITION_DIRECTION = -1 + def feetech_cleanup_handler(): """Cleanup function to ensure Feetech servos are disconnected properly.""" @@ -60,6 +88,8 @@ class FeetechClient(MotorClient): providing compatibility with the OrcaHand control system. """ + waits_for_motion = True + OPEN_CLIENTS = set() def __init__( @@ -99,10 +129,11 @@ def __init__( self._connected = False # Default motion parameters - # Speed unit is 0.732 RPM per value. In practice, motor tops out around - # 1500 steps/sec (~22 RPM) due to hardware limits. - self._default_speed = 60 # Good balance of speed and control - self._default_acc = 50 # Acceleration (0-254) + # Speed unit is 0.732 RPM per value; the motor's firmware caps speed + # to whatever its hardware can sustain, so passing a large value just + # means "go as fast as you can". + self._default_speed = 1500 # Effectively "max speed" for STS-class + self._default_acc = 150 # Acceleration (0-254): faster ramp-up self._default_torque = 500 # Torque limit (0-1000), required for motion self.OPEN_CLIENTS.add(self) @@ -163,6 +194,102 @@ def disconnect(self) -> None: if self in self.OPEN_CLIENTS: self.OPEN_CLIENTS.remove(self) + @staticmethod + def scan_for_motors( + port: str, + id_range: tuple, + baud_rates: Optional[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. + """ + 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) + 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], @@ -241,8 +368,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.""" + def _read_state_per_motor_fallback(self) -> MotorRead: + """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) @@ -257,7 +384,7 @@ def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: 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 + 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', @@ -270,7 +397,7 @@ def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ) if result == COMM_SUCCESS and error == 0: vel_signed = self.packet_handler.scs_tohost(vel_raw, 15) - velocities[i] = vel_signed * self.vel_scale + 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', @@ -290,14 +417,34 @@ def read_pos_vel_cur(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: 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.""" + """Reads the temperature for all motors via a single sync-read packet.""" self._check_connected() temperatures = np.zeros(len(self.motor_ids), dtype=np.float32) + sync_read = GroupSyncRead(self.packet_handler, SMS_STS_PRESENT_TEMPERATURE, 1) + for motor_id in self.motor_ids: + sync_read.addParam(motor_id) + + if sync_read.txRxPacket() != COMM_SUCCESS: + logging.warning('Sync temp read failed, falling back to individual reads') + 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) + if available: + temperatures[i] = float(sync_read.getData(motor_id, SMS_STS_PRESENT_TEMPERATURE, 1)) + else: + logging.warning('Motor %d not available in sync temp read', motor_id) + + return temperatures + + 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( motor_id, SMS_STS_PRESENT_TEMPERATURE @@ -312,6 +459,54 @@ def read_temperature(self) -> np.ndarray: return temperatures + def wait_for_motion_complete( + self, + timeout: float = 5.0, + poll_interval: float = 0.02, + ) -> 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 + once it has settled. + + Args: + timeout: Max seconds to wait before giving up. + poll_interval: Seconds between polls. + + Raises: + MotionTimeoutError: If any motor is still moving when the + timeout elapses. + """ + self._check_connected() + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + sync_read = GroupSyncRead(self.packet_handler, SMS_STS_MOVING, 1) + for motor_id in self.motor_ids: + sync_read.addParam(motor_id) + + if sync_read.txRxPacket() != COMM_SUCCESS: + time.sleep(poll_interval) + continue + + all_stopped = True + for motor_id in self.motor_ids: + available, _ = sync_read.isAvailable(motor_id, SMS_STS_MOVING, 1) + if not available: + all_stopped = False + break + if sync_read.getData(motor_id, SMS_STS_MOVING, 1) != 0: + all_stopped = False + break + + if all_stopped: + return + time.sleep(poll_interval) + + raise MotionTimeoutError( + f'Motors did not settle within {timeout:.1f}s' + ) + def write_desired_pos( self, motor_ids: Sequence[int], @@ -321,42 +516,16 @@ def write_desired_pos( ) -> None: """Writes desired positions to the motors. + Routes through ``write_positions_sync`` so all motors receive their + targets in a single broadcast packet (one round-trip instead of N). + Args: motor_ids: Motor IDs to write to. positions: Target positions in radians. speed: Movement speed (0.732 RPM per unit). Default ~60 = 44 RPM. torque: Torque limit (0-1000). Default 500. Required for motion. """ - self._check_connected() - - if len(motor_ids) != len(positions): - raise ValueError('motor_ids and positions must have the same length') - - speed = speed if speed is not None else self._default_speed - torque = torque if torque is not None else self._default_torque - - for motor_id, pos_rad in zip(motor_ids, positions): - # Convert radians to raw position and clamp to valid range - pos_raw = int(pos_rad / self.pos_scale) - pos_raw = self._clamp_position(pos_raw) - - logging.debug( - 'WritePosEx: motor=%d, pos=%d, speed=%d, acc=%d, torque=%d', - motor_id, pos_raw, speed, self._default_acc, torque - ) - - result, error = self.packet_handler.WritePosEx( - motor_id, - pos_raw, - speed, - self._default_acc, - torque, - ) - if result != COMM_SUCCESS or error != 0: - logging.error( - 'Failed to write position to motor %d: result=%d, error=%d', - motor_id, result, error - ) + self.write_positions_sync(motor_ids, positions, speed=speed, torque=torque) def write_desired_current( self, @@ -409,6 +578,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 @@ -428,7 +607,10 @@ def calibrate_offset(self, motor_id: int, upper: bool = True) -> bool: """ self._check_connected() - # Buffer of 500 (~44°) ensures room for tendon loosening over time + # When POSITION_DIRECTION inverts the read frame, "upper" maps to low raw. + if POSITION_DIRECTION < 0: + upper = not upper + target_position = 3595 if upper else 500 result, error = self.packet_handler.reOfsCal(motor_id, target_position) @@ -445,6 +627,40 @@ def calibrate_offset(self, motor_id: int, upper: bool = True) -> bool: ) return True + @staticmethod + def probe( + port: str, + baudrate: int, + motor_ids: Sequence[int], + ) -> bool: + """Open ``port`` at ``baudrate`` and ping the first and last motor IDs. + + 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 sample: + _, 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: @@ -494,8 +710,8 @@ def write_positions_sync( self.packet_handler.groupSyncWrite.clearParam() for motor_id, pos_rad in zip(motor_ids, positions): - pos_raw = int(pos_rad / self.pos_scale) - pos_raw = self._clamp_position(pos_raw) + # 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( 'SyncWritePosEx: motor=%d, pos=%d, speed=%d, acc=%d, torque=%d', @@ -512,10 +728,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_position_velocity_current(self) -> MotorRead: + """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() @@ -533,7 +749,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() + return self._read_state_per_motor_fallback() for i, motor_id in enumerate(self.motor_ids): available, error = sync_read.isAvailable( @@ -543,11 +759,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 + 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 + 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) @@ -555,7 +771,7 @@ def read_pos_vel_cur_sync(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 143ce3c7..4cea2feb 100644 --- a/orca_core/hardware/motor_client.py +++ b/orca_core/hardware/motor_client.py @@ -9,10 +9,30 @@ """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 +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 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. @@ -20,6 +40,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: @@ -70,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. """ ... @@ -116,6 +140,16 @@ def write_desired_current( """ ... + 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). + 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. + """ + @property def requires_offset_calibration(self) -> bool: """Returns True if this motor type needs offset calibration during joint calibration.""" diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index ab9233af..3cc5e579 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -8,26 +8,32 @@ import dataclasses import math +import os import threading 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, get_and_choose_port, update_yaml - -if TYPE_CHECKING: - from .hardware.dynamixel_client import DynamixelClient - from .hardware.feetech_client import FeetechClient +from .utils.utils import ( + auto_detect_port, + find_single_usb_serial_port, + get_and_choose_port, + motor_type_for_port, + update_yaml, +) from .constants import ( SUPPORTED_MOTOR_TYPES, + MOTOR_BAUD_RATES, MODE_MAP, WRIST_MODE_VALUE, CURRENT_BASED_POSITION, @@ -127,82 +133,143 @@ 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": - from .hardware.dynamixel_client import DynamixelClient + def _create_motor_client( + self, + motor_type: str, + port: str, + baudrate: int, + ) -> MotorClient: + if motor_type == "dynamixel": + return DynamixelClient(self.config.motor_ids, port, baudrate) - return DynamixelClient( - self.config.motor_ids, self.config.port, self.config.baudrate - ) + if motor_type == "feetech": + return FeetechClient(self.config.motor_ids, port, baudrate) - if self.config.motor_type == "feetech": - from .hardware.feetech_client import FeetechClient + raise ValueError( + f"Unknown motor_type: {motor_type}. " + f"Expected one of [{', '.join(SUPPORTED_MOTOR_TYPES)}]." + ) - return FeetechClient( - self.config.motor_ids, self.config.port, self.config.baudrate + 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." ) - - raise ValueError( - f"Unknown motor_type: {self.config.motor_type}. Expected one of [{', '.join(SUPPORTED_MOTOR_TYPES)}]." + # 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. + """ + 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_driver(motor_type, port, baudrate) + 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}", - ) + return False, f"Connection failed on {port}: {str(e)}" - except Exception: - self._motor_client = None + def _persist_resolved_driver(self, motor_type: str, port: str, baudrate: int) -> None: + """Persist auto-detected driver fields to config.yaml. - 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" - - 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)}" + 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, **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. @@ -336,7 +403,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 { @@ -356,7 +423,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 { @@ -366,6 +433,23 @@ 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) -> 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 + (e.g., Dynamixel). Feetech polls a per-motor moving flag. + + Args: + timeout: Max seconds to wait. + + Raises: + MotionTimeoutError: If motors fail to settle within ``timeout``. + """ + if not self._motor_client.waits_for_motion: + return + with self._motor_lock: + 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. @@ -473,7 +557,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 @@ -481,17 +570,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, @@ -516,7 +612,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`` @@ -553,22 +653,37 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: {STEP: len(calibration_sequence) + 1, JOINTS: {WRIST: EXTEND}} ) - # Deep-copy current limits so per-step YAML writes reflect only the - # joints being calibrated, not stale data from a prior incomplete run. + 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. 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() @@ -614,6 +729,7 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: 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 ) @@ -669,9 +785,9 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: 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 @@ -683,7 +799,7 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: ) 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( @@ -696,11 +812,16 @@ def _calibrate(self, force_wrist: bool = False) -> CalibrationResult | None: 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] @@ -1154,9 +1275,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..e2be1657 100644 --- a/orca_core/models/v2/orcahand_right/config.yaml +++ b/orca_core/models/v2/orcahand_right/config.yaml @@ -1,6 +1,5 @@ -port: /dev/cu.usbmodem3101 +port: /dev/cu.usbmodem1101 version: 0.2.1 -baudrate: 1000000 max_current: 300 type: right control_mode: current_based_position 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/__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/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/calibrate.py b/scripts/calibrate.py index 0658340f..b382e652 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 either one.") + + 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: 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 79794625..04e451cb 100755 --- a/scripts/configure_motor_chain.py +++ b/scripts/configure_motor_chain.py @@ -1,272 +1,543 @@ -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 -from orca_core.utils import read_yaml, get_model_path - -DEFAULT_ID, DEFAULT_BAUD = 1, 57600 -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' +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 auto_detect_port, get_and_choose_port, get_model_path, read_yaml + + +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' + +FEETECH, DYNAMIXEL = 'feetech', 'dynamixel' + +# 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__) + + +# --- 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(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 os.path.exists(port) != present: + if time.time() - start > timeout: + print(f"{RED}❌ Timeout waiting for {port}.{RESET}") + start = time.time() + time.sleep(0.3) + 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): + """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(port, present=True) + def play_success_beep(): - """Play a short beep to indicate successful motor configuration.""" + """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) + + +# --- 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) + + +@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: - subprocess.run(['paplay', '/usr/share/sounds/freedesktop/stereo/complete.oga'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) - except: + yield client + finally: try: - subprocess.run(['beep', '-f', '800', '-l', '100'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=1) - except: - print('\a', end='', flush=True) + client.port_handler.closePort() + except Exception: + pass + -def scan_for_default_motor(expected_type: str, port: str) -> bool: +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. + """ + for motor_type, defaults in MOTOR_TYPE_DEFAULTS.items(): + baud, default_id = defaults['default_baud'], defaults['default_id'] + print(f" Probing {motor_type} (ID {default_id} @ {baud})...", end=' ', flush=True) + try: + 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.") + except Exception as e: + print(f"{YELLOW}error ({e}).{RESET}") + return None + + +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 = DynamixelClient([], port=port, baudrate=DEFAULT_BAUD) - motors = client.scan_for_motors(port=port, id_range=(DEFAULT_ID, DEFAULT_ID), baud_rates=[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 = 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] + 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) -> 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: + """Re-program a factory-default motor to ``target_id`` and ``target_baud``.""" try: - client = DynamixelClient([DEFAULT_ID], port=port, baudrate=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}") - client.port_handler.closePort() - return False - client.port_handler.closePort() + 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) - 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() - print(f"{GREEN}✓ Successfully configured the new motor to ID={target_id}, baudrate={target_baud}{RESET}") + + if target_baud != default_baud: + 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 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, xc430_id: int = None) -> list: - """Scan for pre-configured motors and validate sequence.""" - 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 = DynamixelClient([], port=port, baudrate=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]) + + 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 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 - min_xc330_id = 2 if xc430_id is not None else 1 - + 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 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: - break - elif motor_id not in range(min_xc330_id, total_motors + 1): + 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 - 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 + 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}") + print(f"{GREEN}\nFound {len(valid_sequence)} valid, pre-configured motors:{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}") + _print_motor_row(motor_by_id[motor_id]) + 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}") - print(f"{YELLOW}\nExpected sequence for {len(motor_by_id)} connected motors: {RESET}") + _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: - 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") - 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}") + 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) -> 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: + """Re-scan after each configure step to detect dropped or duplicated motors.""" if not configured_ids: - return True + return True try: - client = DynamixelClient([], port=port, baudrate=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 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(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 change_motor_baudrate_only(motor_type: str, motor_id: int, port: str, current_baud: int, new_baud: int) -> bool: + """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: + 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: + 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: + 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) + 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: + logger.error("Error resetting motor %d: %s", motor_id, e) + return False + + +# --- 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, 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. Check connections.{RESET}") + continue + for motor in motors: + _print_motor_row(motor) + action(motor) + else: + known_ids: set[int] = set() + while True: + 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} + 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 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", + ) + + +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", + ) + + +# --- main ------------------------------------------------------------------- + +def _load_config(config_path_arg: str | None) -> dict: + """Resolve the config path and read the yaml, exiting on error.""" + try: + if config_path_arg is None: + config_path = os.path.join(get_model_path(), "config.yaml") + else: + 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}") + sys.exit(1) + return read_yaml(config_path) + except Exception as e: + print(f"{RED}❌ Error loading config: {e}{RESET}") + sys.exit(1) + + +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{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 + + 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 config.yaml. Defaults to the bundled model when omitted.") parser.add_argument( - "config_path", - type=str, - nargs="?", - default=None, - help="Path to the hand config.yaml file.") + "--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() - - try: - config_path = os.path.abspath(args.config_path) if args.config_path else os.path.join(get_model_path(), "config.yaml") - 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 - - 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") + + 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: + _banner(f"🔄 ORCAHAND {label.upper()} CHAIN RESET 🔄") + 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 = ( + 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 {label} motors. " + f"Valid: {[f'{b:,}' for b in valid_baudrates]}{RESET}") + return 1 + _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) + except KeyboardInterrupt: + print(f"\n\n{GREEN}Baudrate change mode stopped.{RESET}\n") + return 0 + + # Full chain configuration. + _banner(f"⚙️ ORCAHAND {label.upper()} CHAIN CONFIGURATION ⚙️") + + has_wrist = 'wrist' in config.get('joint_to_motor_map', {}) + 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 []) + 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] - already_configured = scan_already_configured_motors(PORT, TARGET_BAUD, TOTAL_MOTORS, 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: - 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(" ✓ Motor settings: ID=1, baudrate=57600 (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() + 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_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 '' - - 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}" + 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}" + + print(f"\n{BOLD}{ORANGE}STEP {step}/{total_motors}{RESET}") + if motor_type == FEETECH: + feetech_safe_connect_prompt(port, msg) 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}" - - 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...") + print(msg) - try: - while True: - if scan_for_default_motor(motor_type, PORT): - if configure_default_motor(target_id, PORT, TARGET_BAUD): - configured_ids.append(target_id) - if not verify_all_motors(configured_ids, PORT, TARGET_BAUD, TOTAL_MOTORS): - return 1 - break # Move to next motor - else: - return 1 - else: - # Wait a bit before scanning again - time.sleep(1) - except Exception as e: - print(f"\n ERROR: {e}") - sys.exit(1) + 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) - 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/stress_test.py b/scripts/stress_test.py index cbfdf8bb..f2b1bf4b 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 + +MAX_TEMP = 70 # °C — conservative across Dynamixel XC330/430 and Feetech STS3215 TEMP_CHECK_INTERVAL = 2.0 @@ -20,71 +20,107 @@ 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=NUM_STEPS, + help=f"Interpolation steps per move (default {NUM_STEPS})." + ) + parser.add_argument( + "--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=2.0, + help="Seconds to hold each pose AFTER motion completes (default 2)." + ) args = parser.parse_args() hand = create_hand(args.config_path, use_mock=args.mock) @@ -92,66 +128,31 @@ def main() -> int: 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.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.") + 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 + ) + if args.hold: + time.sleep(args.hold) + + hand.set_joint_positions( + JOINT_CLOSE, num_steps=args.num_steps, step_size=args.step_size + ) + if args.hold: + time.sleep(args.hold) + except KeyboardInterrupt: + print("\nInterrupted.") return 0 finally: shutdown_hand(hand) 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 new file mode 100644 index 00000000..88f4f84b --- /dev/null +++ b/tests/test_connect_resolution.py @@ -0,0 +1,124 @@ +"""Tests for OrcaHand.connect() driver auto-detection.""" + +import dataclasses + +from types import SimpleNamespace + +from orca_core.constants import KNOWN_VIDS +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] + + +# ----- USB VID lookup helpers --------------------------------------------- + +def test_motor_type_for_port_matches_known_vid(patch_comports): + 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_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_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_serial_port("/dev/cu.a", 0x2F5D), + fake_serial_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_serial_port("/dev/cu.usb", 0x2F5D), + ] + ) + assert find_single_usb_serial_port() == "/dev/cu.usb" + + +# ----- _trial_probe ------------------------------------------------------- +# 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 + 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_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_dxl_probe) + ) + monkeypatch.setattr( + feetech_client.FeetechClient, "probe", staticmethod(fake_feetech_probe) + ) + OrcaHand._trial_probe(mock_hand, "/dev/cu.x") + assert "feetech" not in seen_types