Improve feetech driver

docs/pages/orca-core-docs/orca-core-scripts.md

@@ -239,17 +239,21 @@ python scripts/tension.py /path/to/orcahand_v1_left/config.yaml
 </details>
 
 <details>
-<summary><strong>test.py</strong></summary>
+<summary><strong>stress_test.py</strong></summary>
 
-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.
 
 <br><strong>Args:</strong><br>
 <ul>
     <li><strong>config_path</strong> (<strong>str</strong>, optional): Path to the hand config file (e.g., `/path/to/orcahand_v1_right/config.yaml`). If not provided, uses the default config path.</li>
+    <li><strong>--num-steps</strong> (<strong>int</strong>, optional): Interpolation steps per move (default 25).</li>
+    <li><strong>--step-size</strong> (<strong>float</strong>, optional): Sleep between interpolation steps in seconds (default 0.001).</li>
+    <li><strong>--hold</strong> (<strong>float</strong>, optional): Extra seconds to hold each pose AFTER motion completes (default 0).</li>
+    <li><strong>--motion-timeout</strong> (<strong>float</strong>, optional): Max seconds to wait for a pose to be reached (default 5).</li>
 </ul>
 
 <strong>Example:</strong>
 ```bash
-python scripts/test.py
+python scripts/stress_test.py
 ```
 </details>

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.

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."

orca_core/hardware/dynamixel_client.py

@@ -214,6 +214,39 @@ def connect(self):
         # Start with all motors enabled.
         self.set_torque_enabled(self.motor_ids, True)
 
+    @staticmethod
+    def probe(
+        port: str,
+        baudrate: int,
+        motor_ids: Sequence[int],
+        ping_count: int = 2,
+    ) -> bool:
+        """Open ``port`` at ``baudrate`` and ping the first few motor IDs.
+
+        Returns True if any motor responds — i.e. the bus is speaking the
+        Dynamixel Protocol 2.0 at this baudrate. Used at connect time to
+        auto-detect the driver family without enabling torque.
+        """
+        from dynamixel_sdk import PortHandler, PacketHandler, COMM_SUCCESS
+
+        handler = PortHandler(port)
+        try:
+            if not handler.openPort():
+                return False
+            if not handler.setBaudRate(baudrate):
+                return False
+            packet = PacketHandler(PROTOCOL_VERSION)
+            for motor_id in list(motor_ids)[:ping_count]:
+                _, comm, _ = packet.ping(handler, motor_id)
+                if comm == COMM_SUCCESS:
+                    return True
+            return False
+        finally:
+            try:
+                handler.closePort()
+            except Exception:
+                pass
+
     def disconnect(self):
         """Disconnects from the Dynamixel device."""
         if not self.is_connected:
@@ -635,7 +668,7 @@ def __init__(self, client: DynamixelClient, motor_ids: Sequence[int],
                     .format(motor_id))
 
     def read(self, retries: int = 1):
-        """Reads data from the motors."""
+        """Read data via bulk; fall back to per-motor reads on failure."""
         self.client.check_connected()
         success = False
         while not success and retries >= 0:
@@ -644,37 +677,47 @@ 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.
         for motor_id, error in self.operation.motor_errors.items():
             if error & _AlertCaptureBulkRead.ALERT_BIT:
                 self.client._handle_hardware_alert(motor_id)
 
-        errored_ids = []
+        missing_ids = []
         for i, motor_id in enumerate(self.motor_ids):
             # Check if the data is available.
             available = self.operation.isAvailable(motor_id, self.address,
                                                    self.size)
             if not available:
-                errored_ids.append(motor_id)
+                missing_ids.append(motor_id)
                 continue
 
             try:
                 self._update_data(i, motor_id)
             except Exception as e:
                 logging.error(f'Error updating data for motor {motor_id}: {e}')
-                errored_ids.append(motor_id)
+                missing_ids.append(motor_id)
                 continue
 
-        if errored_ids:
-            logging.error('Bulk read data is unavailable for: %s',
-                          str(errored_ids))
+        if missing_ids:
+            logging.warning(
+                'Bulk read missing data for %s; per-motor fallback',
+                missing_ids,
+            )
+            self._read_per_motor_fallback(missing_ids)
 
         return self._get_data()
 
+    def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None:
+        """Read each motor in ``motor_ids`` individually."""
+
     def _initialize_data(self):
         """Initializes the cached data."""
         self._data = np.zeros(len(self.motor_ids), dtype=np.float32)
@@ -729,6 +772,50 @@ def _update_data(self, index: int, motor_id: int):
         self._vel_data[index] = float(vel) * self.vel_scale
         self._cur_data[index] = float(cur) * self.cur_scale
 
+    def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None:
+        """Per-motor reads for position / velocity / current.
+
+        Used when the bulk read fails entirely or when specific motor IDs
+        weren't included in the bulk packet. Cached values are kept for
+        motors whose individual read also fails.
+        """
+        port = self.client.port_handler
+        packet = self.client.packet_handler
+        comm_success = self.client.dxl.COMM_SUCCESS
+        for motor_id in motor_ids:
+            try:
+                idx = self.motor_ids.index(motor_id)
+            except ValueError:
+                continue
+            try:
+                pos_raw, comm, _ = packet.read4ByteTxRx(
+                    port, motor_id, ADDR_PRESENT_POSITION
+                )
+                if comm == comm_success:
+                    self._pos_data[idx] = (
+                        float(unsigned_to_signed(pos_raw, size=4)) * self.pos_scale
+                    )
+                vel_raw, comm, _ = packet.read4ByteTxRx(
+                    port, motor_id, ADDR_PRESENT_VELOCITY
+                )
+                if comm == comm_success:
+                    self._vel_data[idx] = (
+                        float(unsigned_to_signed(vel_raw, size=4)) * self.vel_scale
+                    )
+                cur_raw, comm, _ = packet.read2ByteTxRx(
+                    port, motor_id, ADDR_PRESENT_CURRENT
+                )
+                if comm == comm_success:
+                    self._cur_data[idx] = (
+                        float(unsigned_to_signed(cur_raw, size=2)) * self.cur_scale
+                    )
+            except Exception as e:
+                logging.warning(
+                    'Per-motor pos/vel/cur read failed for motor %d: %s',
+                    motor_id,
+                    e,
+                )
+
     def _get_data(self):
         """Returns a copy of the data."""
         return (self._pos_data.copy(), self._vel_data.copy(),
@@ -746,6 +833,29 @@ def _update_data(self, index: int, motor_id: int):
         raw_val = self.operation.getData(motor_id, self.address, self.size)
         self._temp_data[index] = float(raw_val)
 
+    def _read_per_motor_fallback(self, motor_ids: Sequence[int]) -> None:
+        """Per-motor temperature reads."""
+        port = self.client.port_handler
+        packet = self.client.packet_handler
+        comm_success = self.client.dxl.COMM_SUCCESS
+        for motor_id in motor_ids:
+            try:
+                idx = self.motor_ids.index(motor_id)
+            except ValueError:
+                continue
+            try:
+                raw, comm, _ = packet.read1ByteTxRx(
+                    port, motor_id, ADDR_PRESENT_TEMPERATURE
+                )
+                if comm == comm_success:
+                    self._temp_data[idx] = float(raw)
+            except Exception as e:
+                logging.warning(
+                    'Per-motor temperature read failed for motor %d: %s',
+                    motor_id,
+                    e,
+                )
+
     def _get_data(self):
         return self._temp_data.copy()