diff --git a/.gitignore b/.gitignore index e2ca6a0d..7d946550 100644 --- a/.gitignore +++ b/.gitignore @@ -93,7 +93,6 @@ celerybeat-schedule # Environments .env .venv -uv.lock env/ venv/ ENV/ @@ -124,12 +123,9 @@ urdf/ dev_scripts/ -calibration.yaml +orca_core/models/* # macOS .DS_Store .pytest_cache/ - -# not storing the lockfile (as of now) -uv.lock diff --git a/CLAUDE.md b/CLAUDE.md index bad58f91..80c2a1f2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -48,7 +48,6 @@ docs/ # Documentation - Use conventional commits: `Add feature`, `Fix bug`, `Update docs` - Keep messages concise and descriptive -- Do NOT add `Co-Authored-By` lines --- diff --git a/orca_core/__init__.py b/orca_core/__init__.py index 67315f64..6ba939e3 100644 --- a/orca_core/__init__.py +++ b/orca_core/__init__.py @@ -8,8 +8,10 @@ from .calibration import CalibrationResult from .hand_config import BaseHandConfig from .hand_config import OrcaHandConfig +from .hand_config import OrcaHandTouchConfig from .hand_config import canonical_joint_ids from .hardware_hand import OrcaHand +from .hardware_hand import OrcaHandTouch from .joint_position import OrcaJointPositions from .version import LATEST_VERSION @@ -17,7 +19,9 @@ "CalibrationResult", "BaseHandConfig", "OrcaHandConfig", + "OrcaHandTouchConfig", "OrcaHand", + "OrcaHandTouch", "OrcaJointPositions", "canonical_joint_ids", "LATEST_VERSION", diff --git a/orca_core/constants.py b/orca_core/constants.py index b9c3dbbc..97abe647 100644 --- a/orca_core/constants.py +++ b/orca_core/constants.py @@ -20,6 +20,9 @@ 0x1A86, # QinHeng Electronics CH340 (most Feetech USB adapters) 0x10C4, # Silicon Labs CP210x (some Feetech boards) ], + "tactile_sensor": [ + 0x28E9, # Paxini tactile sensor USB adapter + ], } # Baudrates the connect-time probe will try, per motor family, in priority diff --git a/orca_core/hand_config.py b/orca_core/hand_config.py index 4662720a..ccb54cca 100644 --- a/orca_core/hand_config.py +++ b/orca_core/hand_config.py @@ -6,6 +6,7 @@ # See the LICENSE file at the root of this repository for full license information. # ============================================================================== +import dataclasses import os from dataclasses import dataclass, field from typing import Dict, List, Literal, Optional @@ -19,6 +20,13 @@ MOTOR_IDS, SUPPORTED_MOTOR_TYPES, ) +from .hardware.sensing.constants import ( + FINGER_NAMES, + VALID_SENSOR_IDS, + DEFAULT_SENSOR_PORT, + DEFAULT_SENSOR_BAUDRATE, + DEFAULT_FINGER_TO_SENSOR_ID, +) from .joint_position import OrcaJointPositions from .utils.utils import get_model_path, read_yaml @@ -336,4 +344,59 @@ def __post_init__(self) -> None: self.validate_config() +@dataclass(frozen=True) +class OrcaHandTouchConfig(OrcaHandConfig): + """ORCA hand configuration with tactile sensor support.""" + + sensor_port: str = DEFAULT_SENSOR_PORT + sensor_baudrate: int = DEFAULT_SENSOR_BAUDRATE + finger_to_sensor_id: Dict[str, int] = field( + default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID) + ) + + @classmethod + def from_config_path( + cls, + config_path: str | None = None, + calibration_path: str | None = None, + model_version: str | None = None, + model_name: str | None = None, + ) -> "OrcaHandTouchConfig": + base = OrcaHandConfig.from_config_path( + config_path=config_path, + calibration_path=calibration_path, + model_version=model_version, + model_name=model_name, + ) + + config = read_yaml(base.config_path) + sensors = config.get("sensors", {}) + + sensor_kwargs = {} + if "port" in sensors: + sensor_kwargs["sensor_port"] = sensors["port"] + if "baudrate" in sensors: + sensor_kwargs["sensor_baudrate"] = int(sensors["baudrate"]) + if "finger_to_sensor_id" in sensors: + sensor_kwargs["finger_to_sensor_id"] = dict(sensors["finger_to_sensor_id"]) + + return cls(**{f.name: getattr(base, f.name) for f in dataclasses.fields(base)}, **sensor_kwargs) + + def validate_config(self) -> None: + super().validate_config() + + if set(self.finger_to_sensor_id.keys()) != set(FINGER_NAMES): + raise HandConfigValidationError( + f"finger_to_sensor_id must contain exactly {FINGER_NAMES}, " + f"got {sorted(self.finger_to_sensor_id.keys())}" + ) + + ids = set(self.finger_to_sensor_id.values()) + if ids != VALID_SENSOR_IDS: + raise HandConfigValidationError( + f"finger_to_sensor_id values must be 0-4 with no duplicates, " + f"got {sorted(self.finger_to_sensor_id.values())}" + ) + + HandConfig = BaseHandConfig diff --git a/orca_core/hardware/mock_tactile_client.py b/orca_core/hardware/mock_tactile_client.py new file mode 100644 index 00000000..1e5dedd5 --- /dev/null +++ b/orca_core/hardware/mock_tactile_client.py @@ -0,0 +1,283 @@ +# ============================================================================== +# Copyright (c) 2025 ORCA Dexterity, Inc. All rights reserved. +# +# This file is part of ORCA Dexterity and is licensed under the MIT License. +# You may use, copy, modify, and distribute this file under the terms of the MIT License. +# See the LICENSE file at the root of this repository for full license information. +# ============================================================================== +"""Mock client for simulating tactile sensors in automated tests.""" + +from __future__ import annotations + +from collections.abc import Callable +import threading +import logging + +from orca_core.hardware.tactile_client import TactileClient +from orca_core.hardware.sensing.constants import ( + FINGER_NAMES, + DEFAULT_TAXEL_COUNTS, + DEFAULT_SENSOR_BAUDRATE, + AUTO_DATA_RESULTANT, + AUTO_DATA_TAXELS, +) +from orca_core.hardware.sensing.protocol import ( + ResultantForces, + TaxelForces, + decode_combined_auto, + decode_resultant_auto, + decode_taxels_auto, + encode_combined_auto_for_mock, + encode_resultant_auto_for_mock, + encode_taxels_auto_for_mock, +) + +logger = logging.getLogger(__name__) + +ResultantProvider = Callable[[], ResultantForces] +TaxelProvider = Callable[[], TaxelForces] + + +class MockTactileClient(TactileClient): + """Mock client for simulating tactile sensor communication in tests. + + Subclasses TactileClient, replacing hardware I/O with deterministic data + sources. Inherits start_auto_stream, stop_auto_stream, offset logic, and + context manager support from the base class. + + Control simulated data via: + - set_mock_forces(): Set specific force values to return + - set_mock_taxels(): Set specific taxel values to return + - set_resultant_provider()/set_taxel_provider(): Inject deterministic generators + + Default behavior (if no providers or mock values are set): + - All force components return 1.0 for every connected sensor/taxel. + """ + + def __init__( + self, + port: str = "mock", + baudrate: int = DEFAULT_SENSOR_BAUDRATE, + connected_sensors: list[str] | None = None, + taxel_counts: dict[str, int] | None = None, + finger_to_sensor_id: dict[str, int] | None = None, + resultant_provider: ResultantProvider | None = None, + taxel_provider: TaxelProvider | None = None, + ): + super().__init__(port=port, baudrate=baudrate, finger_to_sensor_id=finger_to_sensor_id) + + if connected_sensors is None: + connected_sensors = list(FINGER_NAMES) + + full_taxel_counts = dict(taxel_counts) if taxel_counts is not None else dict(DEFAULT_TAXEL_COUNTS) + + self._sim_connected: dict[str, bool] = { + f: f in connected_sensors for f in FINGER_NAMES + } + self._sim_taxel_counts: dict[str, int] = { + f: full_taxel_counts[f] if f in connected_sensors else 0 + for f in FINGER_NAMES + } + + self._mock_forces: ResultantForces = {} + self._mock_taxels: TaxelForces = {} + + self._resultant_provider: ResultantProvider = ( + resultant_provider if resultant_provider is not None else self._default_resultant_provider + ) + self._taxel_provider: TaxelProvider = ( + taxel_provider if taxel_provider is not None else self._default_taxel_provider + ) + + # Lets tests synchronize on stream start without polling. + self._first_frame_event = threading.Event() + + # ========================================================================= + # Mock Control Methods + # ========================================================================= + + def set_mock_forces(self, forces: ResultantForces) -> None: + """Replace the resultant-force values returned by ``_default_resultant_provider``.""" + self._validate_finger_vectors(forces, expected_len=3, label="Force") + self._mock_forces = {f: list(v) for f, v in forces.items()} + + def set_mock_taxels(self, taxels: TaxelForces) -> None: + """Replace the taxel values returned by ``_default_taxel_provider``.""" + for finger, taxel_list in taxels.items(): + for taxel in taxel_list: + self._validate_finger_vectors({finger: taxel}, expected_len=3, label="Taxel") + self._mock_taxels = {f: [list(t) for t in data] for f, data in taxels.items()} + + def set_resultant_provider(self, provider: ResultantProvider) -> None: + """Inject a deterministic resultant provider (called per read).""" + self._resultant_provider = provider + + def set_taxel_provider(self, provider: TaxelProvider) -> None: + """Inject a deterministic taxel provider (called per read).""" + self._taxel_provider = provider + + # ========================================================================= + # Connection (no-op hardware I/O) + # ========================================================================= + + def connect(self) -> None: + if self.is_connected: + return + self._connected = True + self._tactile_config = self._get_configuration() + logger.info(f"[MOCK] Connected, config: {self._tactile_config}") + + def disconnect(self) -> None: + if not self.is_connected: + return + # stop_auto_stream must precede _connected = False: the base class + # stop_auto_stream calls disable_auto_data_transmission which checks + # is_connected via _write_register. + self.stop_auto_stream() + self._connected = False + logger.info("[MOCK] Disconnected") + + # ========================================================================= + # Sensor Information (return simulated state) + # ========================================================================= + + def read_connected_sensors(self) -> dict[str, bool]: + if not self.is_connected: + raise OSError("Must call connect() first.") + return dict(self._sim_connected) + + def read_num_taxels(self) -> dict[str, int]: + if not self.is_connected: + raise OSError("Must call connect() first.") + return dict(self._sim_taxel_counts) + + def read_auto_data_type(self) -> dict: + if not self.is_connected: + raise OSError("Must call connect() first.") + val = (AUTO_DATA_RESULTANT if self._auto_mode_resultant else 0) | ( + AUTO_DATA_TAXELS if self._auto_mode_taxels else 0 + ) + return { + "raw": f"{val:08b}", + "resultant": self._auto_mode_resultant, + "taxels": self._auto_mode_taxels, + } + + # ========================================================================= + # Hardware I/O overrides (no-ops) + # ========================================================================= + + def _write_register(self, address: int, data: bytes, response_timeout_s: float = 0.5) -> None: + pass + + # ========================================================================= + # Force Reading + # ========================================================================= + + def _read_raw_resultant(self) -> ResultantForces: + forces = self._resultant_provider() + active = self._active_in_slot_order(forces) + return decode_resultant_auto( + encode_resultant_auto_for_mock(forces, active), active, + ) + + def _active_in_slot_order(self, forces: dict) -> list[str]: + """Sort the provider's fingers by hardware slot ID, matching TactileSensorConfiguration.""" + return sorted( + forces.keys(), + key=lambda f: self._finger_to_sensor_id.get(f, FINGER_NAMES.index(f)), + ) + + # ========================================================================= + # Frame Acquisition (overrides base class serial reader) + # ========================================================================= + + def start_auto_stream(self, *args, **kwargs): + self._first_frame_event.clear() + super().start_auto_stream(*args, **kwargs) + + def wait_for_first_frame(self, timeout: float = 2.0) -> None: + """Block until the auto-stream loop has stored its first frame, or raise ``TimeoutError``.""" + if not self._first_frame_event.wait(timeout): + raise TimeoutError(f"No auto-stream frame within {timeout}s") + + def _on_frame_stored(self) -> None: + self._first_frame_event.set() + + def _acquire_frame( + self, + parse_resultant: bool, + parse_taxels: bool, + ) -> tuple[dict | None, dict | None]: + """Acquire simulated frame data, round-tripped through the real wire codec. + + Provider output is encoded into wire bytes and decoded back. This + ensures mock-driven tests exercise the actual protocol decoders + instead of bypassing them. + """ + active = self._tactile_config.active_sensors + num_taxels = self._tactile_config.num_taxels + + if parse_resultant and parse_taxels: + forces = self._resultant_provider() + taxels = self._taxel_provider() + wire = encode_combined_auto_for_mock(forces, taxels, active) + parsed_resultant, parsed_taxels = decode_combined_auto(wire, active, num_taxels) + elif parse_resultant: + forces = self._resultant_provider() + wire = encode_resultant_auto_for_mock(forces, active) + parsed_resultant = decode_resultant_auto(wire, active) + parsed_taxels = None + elif parse_taxels: + taxels = self._taxel_provider() + wire = encode_taxels_auto_for_mock(taxels, active) + parsed_resultant = None + parsed_taxels = decode_taxels_auto(wire, active, num_taxels) + else: + parsed_resultant = None + parsed_taxels = None + + return parsed_resultant, parsed_taxels + + # ========================================================================= + # Internal Helpers + # ========================================================================= + + def _default_resultant_provider(self) -> ResultantForces: + forces = self._mock_forces + return { + f: list(forces[f]) if f in forces else [1.0, 1.0, 1.0] + for f in FINGER_NAMES + if self._sim_connected.get(f, False) + } + + def _default_taxel_provider(self) -> TaxelForces: + result = {} + for finger in FINGER_NAMES: + if not self._sim_connected.get(finger, False): + continue + expected = self._sim_taxel_counts.get(finger, 0) + if finger in self._mock_taxels: + provided = self._mock_taxels[finger] + if len(provided) != expected: + raise ValueError( + f"Mock taxel count for '{finger}' is {len(provided)}, " + f"but sensor expects {expected}" + ) + result[finger] = [list(t) for t in provided] + else: + result[finger] = [[1.0, 1.0, 1.0] for _ in range(expected)] + return result + + @staticmethod + def _validate_finger_vectors( + data: ResultantForces, expected_len: int, label: str + ) -> None: + for finger, vec in data.items(): + if finger not in FINGER_NAMES: + raise ValueError(f"Unknown finger '{finger}'. Valid names: {FINGER_NAMES}") + if len(vec) != expected_len: + raise ValueError( + f"{label} vector for '{finger}' must have {expected_len} components, " + f"got {len(vec)}" + ) diff --git a/orca_core/hardware/sensing/__init__.py b/orca_core/hardware/sensing/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py new file mode 100644 index 00000000..9bbf8f73 --- /dev/null +++ b/orca_core/hardware/sensing/constants.py @@ -0,0 +1,108 @@ +"""Constants for ORCA tactile sensing.""" + +from typing import Literal + +# --------------------------------------------------------------------------- +# Client configuration defaults +# --------------------------------------------------------------------------- + +FingerName = Literal["thumb", "index", "middle", "ring", "pinky"] +"""Type alias for valid finger names. Use in public APIs that take a single +finger name so type checkers flag typos like ``reading["thmub"]``.""" + +FINGER_NAMES: list[FingerName] = ["thumb", "index", "middle", "ring", "pinky"] +VALID_SENSOR_IDS = set(range(5)) +DEFAULT_SENSOR_PORT = "/dev/ttyACM1" +DEFAULT_SENSOR_BAUDRATE = 921600 +DEFAULT_FINGER_TO_SENSOR_ID = { + "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4, +} + +# Default taxel counts per finger (must match sensor model configs) +DEFAULT_TAXEL_COUNTS = { + "thumb": 51, "index": 87, "middle": 87, "ring": 87, "pinky": 51, +} + +FINGER_MODELS = { + "thumb": "touch-sensor-thumb", + "index": "touch-sensor-finger", + "middle": "touch-sensor-finger", + "ring": "touch-sensor-finger", + "pinky": "touch-sensor-pinky", +} + +# --------------------------------------------------------------------------- +# Protocol wire format (shared between I/O layer and codec) +# --------------------------------------------------------------------------- + +PROTOCOL_HEADER_REQUEST = bytes([0x55, 0xAA]) +PROTOCOL_HEADER_RESPONSE = bytes([0xAA, 0x55]) +PROTOCOL_HEADER_AUTO = bytes([0xAA, 0x56]) +PROTOCOL_RESERVED = 0x00 +FUNC_CODE_READ = 0x03 +FUNC_CODE_WRITE = 0x10 + +# --------------------------------------------------------------------------- +# Register addresses (used by tactile_client for read/write targets) +# --------------------------------------------------------------------------- + +ADDR_RESET = 0x0022 +ADDR_CONNECTED_SENSORS_START = 0x0010 +ADDR_CONNECTED_SENSORS_LENGTH = 4 +ADDR_NUM_TAXELS_START = 0x0030 +ADDR_NUM_TAXELS_LENGTH = 56 +ADDR_RESULTANT_FORCE_START = 0x0500 +RESULTANT_BLOCK_SIZE = 168 +ADDR_AUTO_DATA_TYPE = 0x0016 +ADDR_AUTO_ENABLE = 0x0017 + +# Register write values +REGISTER_ENABLE = bytes([0x01]) +REGISTER_DISABLE = bytes([0x00]) + +# --------------------------------------------------------------------------- +# Codec internals (used by protocol.py decoders) +# --------------------------------------------------------------------------- + +# Auto data type bitmasks +AUTO_DATA_RESULTANT = 0x01 +AUTO_DATA_TAXELS = 0x02 + +# Force resolution +RESOLUTION_N_PER_LSB = 0.1 + +# Byte sizes per data element +BYTES_PER_RESULTANT = 6 # 3 axes × 2-byte slot (low byte = data, high byte = padding) +BYTES_PER_TAXEL = 3 # fx(int8) + fy(int8) + fz(uint8) + +# Frame metadata sizes +RESPONSE_META_SIZE = 6 +"""Bytes between response header and variable data: reserved(1) + func(1) + addr(2) + count/nbytes(2).""" + +AUTO_FRAME_META_SIZE = 3 +"""Bytes between auto-stream header and payload: reserved(1) + eff_len(2).""" + +# Minimum valid frame sizes +MIN_READ_RESPONSE_SIZE = 9 +"""Minimum valid read response frame: header(2) + meta(6) + LRC(1).""" + +MIN_WRITE_RESPONSE_SIZE = 9 +"""Minimum valid write response frame: header(2) + meta(6) + LRC(1).""" + +# Maximum valid effective frame length in auto-stream frames. +# Typical payloads are 6-200 bytes; this guards against corrupted eff_len fields. +MAX_AUTO_FRAME_EFF_LEN = 8192 + +# Register block structure +MODULES_PER_SLOT = 4 +"""Modules per sensor slot in the resultant force register block (proximal, middle, distal, nail).""" + +DISTAL_MODULE_OFFSET = 2 +"""Offset of the distal phalanx module within a slot's module group.""" + +# Slot bit positions (byte_idx, bit_pos) in the connected-sensors register. +# Hardware-fixed; finger mapping is applied on top via finger_to_sensor_id. +SLOT_CONNECTED_BIT_POSITIONS = [(0, 2), (0, 6), (1, 2), (1, 6), (2, 2)] + +# Register address per slot for distal-phalanx taxel count. Hardware-fixed. +SLOT_DISTAL_TAXEL_REGISTER_OFFSETS = [0x0034, 0x003C, 0x0044, 0x004C, 0x0054] diff --git a/orca_core/hardware/sensing/protocol.py b/orca_core/hardware/sensing/protocol.py new file mode 100644 index 00000000..69b339c6 --- /dev/null +++ b/orca_core/hardware/sensing/protocol.py @@ -0,0 +1,544 @@ +"""Binary protocol codec for ORCA tactile sensor communication. + +Converts between raw bytes (as defined by the sensor hardware protocol) +and Python objects. Pure functions only — no I/O, no state, no threading. +""" +from __future__ import annotations + +from typing import TypedDict + +from orca_core.hardware.sensing.constants import ( + PROTOCOL_HEADER_REQUEST, + PROTOCOL_HEADER_RESPONSE, + PROTOCOL_HEADER_AUTO, + PROTOCOL_RESERVED, + FUNC_CODE_READ, + FUNC_CODE_WRITE, + AUTO_DATA_RESULTANT, + AUTO_DATA_TAXELS, + ADDR_NUM_TAXELS_START, + ADDR_NUM_TAXELS_LENGTH, + RESULTANT_BLOCK_SIZE, + RESOLUTION_N_PER_LSB, + BYTES_PER_RESULTANT, + BYTES_PER_TAXEL, + SLOT_CONNECTED_BIT_POSITIONS, + SLOT_DISTAL_TAXEL_REGISTER_OFFSETS, + MAX_AUTO_FRAME_EFF_LEN, + RESPONSE_META_SIZE, + AUTO_FRAME_META_SIZE, + MODULES_PER_SLOT, + DISTAL_MODULE_OFFSET, + MIN_READ_RESPONSE_SIZE, + MIN_WRITE_RESPONSE_SIZE, +) + +# ========================================================================= +# Types +# ========================================================================= + +ForceVector = list[float] +"""[fx, fy, fz] force components in Newtons. Always exactly 3 elements. +fx/fy are signed (shear); fz is unsigned (normal force, always >= 0). +Mutable list (not tuple) because callers apply zeroing offsets in-place.""" + +ResultantForces = dict[str, ForceVector] +"""{finger_name: [fx, fy, fz]} resultant forces per sensor.""" + +TaxelForces = dict[str, list[ForceVector]] +"""{finger_name: [[fx, fy, fz], ...]} per-taxel forces per sensor.""" + + +class AutoDataTypeInfo(TypedDict): + """Parsed auto-data-type register value.""" + raw: str + resultant: bool + taxels: bool + + + + +# ========================================================================= +# Checksum +# ========================================================================= + +def calculate_checksum(frame: bytes) -> int: + """LRC checksum: two's complement of the low byte of the sum.""" + return (0x100 - (sum(frame) & 0xFF)) & 0xFF + + +def validate_auto_frame_lrc(meta: bytes, payload: bytes, lrc: int) -> bool: + """Return ``True`` if ``lrc`` matches the checksum over ``header + meta + payload``. + + Returns bool rather than raising because the auto-stream reader treats a + bad LRC as a recoverable counter, not an abort condition. + """ + frame_without_lrc = PROTOCOL_HEADER_AUTO + meta + payload + return calculate_checksum(frame_without_lrc) == lrc + + +def _validate_frame_lrc(frame: bytes, context: str) -> None: + if frame[-1] != calculate_checksum(frame[:-1]): + raise IOError(f"{context} LRC mismatch") + + +# ========================================================================= +# Frame Size Helpers +# ========================================================================= + +def read_response_body_size(count: int) -> int: + """Bytes after the AA55 header in a read response with ``count`` data bytes.""" + return RESPONSE_META_SIZE + count + 1 # meta + data + LRC + + +# ========================================================================= +# Frame Builders +# ========================================================================= + +def _validate_u16(value: int, name: str) -> None: + if not 0 <= value <= 0xFFFF: + raise ValueError(f"{name} must be 0x0000-0xFFFF, got {value}") + + +def build_read_request(address: int, count: int) -> bytes: + """Build a read-register request frame (55 AA | 00 | 03 | addr | count | LRC).""" + _validate_u16(address, "address") + if count <= 0: + raise ValueError(f"count must be > 0, got {count}") + _validate_u16(count, "count") + body = ( + PROTOCOL_HEADER_REQUEST + + bytes([PROTOCOL_RESERVED, FUNC_CODE_READ]) + + address.to_bytes(2, "little") + + count.to_bytes(2, "little") + ) + return body + bytes([calculate_checksum(body)]) + + +def build_write_request(address: int, data: bytes) -> bytes: + """Build a write-register request frame (55 AA | 00 | 10 | addr | len | data | LRC).""" + _validate_u16(address, "address") + if len(data) == 0: + raise ValueError("data must not be empty") + _validate_u16(len(data), "data length") + body = ( + PROTOCOL_HEADER_REQUEST + + bytes([PROTOCOL_RESERVED, FUNC_CODE_WRITE]) + + address.to_bytes(2, "little") + + len(data).to_bytes(2, "little") + + data + ) + return body + bytes([calculate_checksum(body)]) + + +# ========================================================================= +# Frame Parsers — response frames (request-response mode) +# ========================================================================= + +def parse_read_response(frame: bytes) -> bytes: + """Validate a read-response frame and return its data bytes. + + Frame layout: header(2) + reserved(1) + func(1) + addr(2) + count(2) + data(count) + LRC(1). + Raises ``IOError`` on size, func-code, or LRC mismatch. + """ + if len(frame) < MIN_READ_RESPONSE_SIZE: + raise IOError( + f"Read response frame too short: {len(frame)} bytes " + f"(minimum {MIN_READ_RESPONSE_SIZE}), data={frame.hex()}" + ) + if frame[:2] != PROTOCOL_HEADER_RESPONSE: + raise IOError( + f"Expected response header {PROTOCOL_HEADER_RESPONSE.hex()}, " + f"got {frame[:2].hex()}" + ) + _validate_frame_lrc(frame, "Read response") + if frame[3] != FUNC_CODE_READ: + raise IOError( + f"Expected read response (func=0x{FUNC_CODE_READ:02X}), " + f"got func=0x{frame[3]:02X}" + ) + declared_count = int.from_bytes(frame[6:8], "little") + actual_data = frame[8:-1] + if len(actual_data) != declared_count: + raise IOError( + f"Read response length mismatch: header declares {declared_count} bytes " + f"but frame contains {len(actual_data)}, data={frame.hex()}" + ) + return bytes(actual_data) + + +def extract_write_response_data_length(meta: bytes) -> int: + """Read the payload length from the 6-byte write-response meta block.""" + if len(meta) != RESPONSE_META_SIZE: + raise ValueError(f"Write response meta must be {RESPONSE_META_SIZE} bytes, got {len(meta)}") + return int.from_bytes(meta[4:6], "little") + + +def parse_write_response(frame: bytes) -> None: + """Validate a write-response frame's LRC and status byte. + + Frame layout: header(2) + reserved(1) + func(1) + addr(2) + nbytes(2) + payload(nbytes) + LRC(1). + Raises ``IOError`` on size, LRC, or non-zero status. + """ + if len(frame) < MIN_WRITE_RESPONSE_SIZE: + raise IOError( + f"Write response frame too short: {len(frame)} bytes " + f"(minimum {MIN_WRITE_RESPONSE_SIZE}), data={frame.hex()}" + ) + if frame[:2] != PROTOCOL_HEADER_RESPONSE: + raise IOError( + f"Expected response header {PROTOCOL_HEADER_RESPONSE.hex()}, " + f"got {frame[:2].hex()}" + ) + _validate_frame_lrc(frame, "Write response") + nbytes = int.from_bytes(frame[6:8], "little") + if nbytes >= 1: + if len(frame) < 9 + nbytes: + raise IOError( + f"Write response frame truncated: claims {nbytes} payload bytes " + f"but frame is only {len(frame)} bytes, data={frame.hex()}" + ) + status = frame[8] + if status != 0: + raise IOError(f"Write failed, status=0x{status:02X}") + + +# ========================================================================= +# Frame Parsers — auto-stream frames +# ========================================================================= + +def extract_auto_frame_eff_len(meta: bytes) -> int: + """Read eff_len (includes error_code byte) from the 3-byte auto-frame meta. + + Raises ``ValueError`` if it exceeds ``MAX_AUTO_FRAME_EFF_LEN``, which would + almost always indicate stream corruption rather than a real giant payload. + """ + eff_len = int.from_bytes(meta[1:3], "little") + if eff_len > MAX_AUTO_FRAME_EFF_LEN: + raise ValueError(f"Invalid eff_len in auto frame: {eff_len} (possible corruption)") + return eff_len + + +def unpack_auto_payload(payload: bytes) -> tuple[int, bytes]: + """Split an auto-stream payload into (error_code, force_data). + + The protocol prefixes force data with a single error-code byte + (0 = no error). Empty payloads raise ``ValueError``. + """ + if len(payload) == 0: + raise ValueError("Auto-stream payload is empty (expected at least error code byte)") + return payload[0], payload[1:] + + +# ========================================================================= +# Payload Size Computation +# ========================================================================= + +def compute_resultant_payload_size(num_sensors: int) -> int: + return num_sensors * BYTES_PER_RESULTANT + + +def compute_taxel_payload_size( + active_sensors: list[str], num_taxels: dict[str, int], +) -> int: + return sum(num_taxels[f] for f in active_sensors) * BYTES_PER_TAXEL + + +def compute_combined_payload_size( + active_sensors: list[str], num_taxels: dict[str, int], +) -> int: + return ( + compute_resultant_payload_size(len(active_sensors)) + + compute_taxel_payload_size(active_sensors, num_taxels) + ) + + +def compute_expected_payload_size( + mode_resultant: bool, + mode_taxels: bool, + active_sensors: list[str], + num_taxels: dict[str, int], +) -> int: + """Dispatch to the right ``compute_*_payload_size`` for the active mode.""" + if mode_resultant and mode_taxels: + return compute_combined_payload_size(active_sensors, num_taxels) + elif mode_resultant: + return compute_resultant_payload_size(len(active_sensors)) + elif mode_taxels: + return compute_taxel_payload_size(active_sensors, num_taxels) + return 0 + + +# ========================================================================= +# Payload Decoders — auto-stream formats +# ========================================================================= + +def _validate_payload_size(data: bytes, expected: int, context: str) -> None: + if len(data) != expected: + preview = data[:16].hex() if data else "(empty)" + raise ValueError( + f"{context} size mismatch: expected {expected} bytes, " + f"got {len(data)} bytes (first bytes: {preview})" + ) + + +def _unpack_taxel(data: bytes, offset: int) -> ForceVector: + """Unpack one taxel force vector: 3 contiguous bytes (int8 fx, int8 fy, uint8 fz).""" + fx_byte, fy_byte, fz_byte = data[offset], data[offset + 1], data[offset + 2] + fx = (fx_byte - 256 if fx_byte > 127 else fx_byte) * RESOLUTION_N_PER_LSB + fy = (fy_byte - 256 if fy_byte > 127 else fy_byte) * RESOLUTION_N_PER_LSB + fz = fz_byte * RESOLUTION_N_PER_LSB + return [round(fx, 1), round(fy, 1), round(fz, 1)] + + +def _unpack_resultant(data: bytes, offset: int) -> ForceVector: + """Unpack one resultant force vector from a 6-byte module slot. + + Each axis occupies a 2-byte slot; only the low byte carries data + (signed int8 for fx/fy, unsigned uint8 for fz). The high byte is + sign-extension of the low byte cast to int8 and must be discarded. + """ + fx_lo, fy_lo, fz_lo = data[offset], data[offset + 2], data[offset + 4] + fx = (fx_lo - 256 if fx_lo > 127 else fx_lo) * RESOLUTION_N_PER_LSB + fy = (fy_lo - 256 if fy_lo > 127 else fy_lo) * RESOLUTION_N_PER_LSB + fz = fz_lo * RESOLUTION_N_PER_LSB + return [round(fx, 1), round(fy, 1), round(fz, 1)] + + +def decode_resultant_auto( + data: bytes, + active_sensors: list[str], +) -> ResultantForces: + """Decode auto-stream resultant forces (6 bytes/sensor, in slot order). + + ``active_sensors`` must already be sorted by hardware slot ID ascending — + the codec cannot validate this and assumes the caller has done so. + """ + expected_size = len(active_sensors) * BYTES_PER_RESULTANT + _validate_payload_size(data, expected_size, f"Resultant auto ({len(active_sensors)} sensors)") + + result = {} + for i, finger in enumerate(active_sensors): + result[finger] = _unpack_resultant(data, i * BYTES_PER_RESULTANT) + return result + + +def decode_taxels_auto( + data: bytes, + active_sensors: list[str], + num_taxels: dict[str, int], +) -> TaxelForces: + """Decode auto-stream taxel data (3 bytes/taxel, sequential by sensor). + + ``active_sensors`` must already be in slot order (see ``decode_resultant_auto``). + """ + expected_size = compute_taxel_payload_size(active_sensors, num_taxels) + _validate_payload_size(data, expected_size, "Taxels auto") + + result = {} + offset = 0 + for finger in active_sensors: + finger_taxels = [] + for _ in range(num_taxels[finger]): + finger_taxels.append(_unpack_taxel(data, offset)) + offset += BYTES_PER_TAXEL + result[finger] = finger_taxels + return result + + +def decode_combined_auto( + data: bytes, + active_sensors: list[str], + num_taxels: dict[str, int], +) -> tuple[ResultantForces, TaxelForces]: + """Decode interleaved auto-stream: per sensor, resultant(6) + taxels(3 each). + + ``active_sensors`` must already be in slot order (see ``decode_resultant_auto``). + """ + expected_size = compute_combined_payload_size( + active_sensors, num_taxels, + ) + _validate_payload_size(data, expected_size, "Combined auto") + + offset = 0 + resultant_forces = {} + taxels = {} + + for finger in active_sensors: + resultant_forces[finger] = _unpack_resultant(data, offset) + offset += BYTES_PER_RESULTANT + + finger_taxels = [] + for _ in range(num_taxels[finger]): + finger_taxels.append(_unpack_taxel(data, offset)) + offset += BYTES_PER_TAXEL + taxels[finger] = finger_taxels + + return resultant_forces, taxels + + +# ========================================================================= +# Payload Decoders — register block format (request-response mode) +# ========================================================================= + +def compute_distal_module_index(sensor_id: int) -> int: + """Module index of the distal phalanx for ``sensor_id`` (slot 0-4). + + The register block packs ``MODULES_PER_SLOT`` modules per slot + (proximal, middle, distal, nail); the distal one sits at + ``DISTAL_MODULE_OFFSET`` inside each group. + """ + return sensor_id * MODULES_PER_SLOT + DISTAL_MODULE_OFFSET + + +def decode_resultant_register( + data: bytes, + active_sensors: list[str], + module_indices: dict[str, int], +) -> ResultantForces: + """Decode the 168-byte resultant register block (0x0500-0x05A7) at given indices. + + The block packs 28 modules of 6 bytes each (4 per slot + 8 palm). + ``module_indices[finger]`` is the zero-based module index; for fingertip + sensors use ``compute_distal_module_index(slot_id)`` to derive it. + """ + if len(data) < RESULTANT_BLOCK_SIZE: + raise ValueError(f"Resultant force block too short: {len(data)} bytes") + + result = {} + for finger in active_sensors: + result[finger] = _unpack_resultant(data, module_indices[finger] * BYTES_PER_RESULTANT) + return result + + +# ========================================================================= +# Register Codecs +# ========================================================================= + +def decode_connected_sensors( + data: bytes, + sensor_id_to_finger: dict[int, str], +) -> dict[str, bool]: + """Decode the 4-byte connected-sensors register into ``{finger: is_connected}``.""" + num_slots = len(SLOT_CONNECTED_BIT_POSITIONS) + if len(data) < 4: + raise ValueError(f"Expected 4 bytes, got {len(data)}") + if len(sensor_id_to_finger) != num_slots: + raise ValueError( + f"sensor_id_to_finger must have {num_slots} entries, " + f"got {len(sensor_id_to_finger)}" + ) + status = [bool(data[byte_idx] & (1 << bit_pos)) for byte_idx, bit_pos in SLOT_CONNECTED_BIT_POSITIONS] + return {sensor_id_to_finger[i]: status[i] for i in range(num_slots)} + + +def decode_num_taxels( + data: bytes, + sensor_id_to_finger: dict[int, str], +) -> dict[str, int]: + """Decode the 56-byte taxel-count register block (28 × uint16 LE) into ``{finger: count}``.""" + num_slots = len(SLOT_DISTAL_TAXEL_REGISTER_OFFSETS) + if len(data) != ADDR_NUM_TAXELS_LENGTH: + raise ValueError( + f"Taxel count register block size mismatch: " + f"expected {ADDR_NUM_TAXELS_LENGTH} bytes, got {len(data)}" + ) + if len(sensor_id_to_finger) != num_slots: + raise ValueError( + f"sensor_id_to_finger must have {num_slots} entries, " + f"got {len(sensor_id_to_finger)}" + ) + taxel_counts = [ + int.from_bytes(data[i:i+2], byteorder="little") + for i in range(0, len(data), 2) + ] + num_entries = len(taxel_counts) + distal_indices = {} + for slot, addr in enumerate(SLOT_DISTAL_TAXEL_REGISTER_OFFSETS): + idx = (addr - ADDR_NUM_TAXELS_START) // 2 + if idx < 0 or idx >= num_entries: + raise ValueError( + f"Slot {slot} register offset {addr:#x} maps to index {idx}, " + f"but register block only has {num_entries} entries" + ) + distal_indices[sensor_id_to_finger[slot]] = idx + return {finger: taxel_counts[idx] for finger, idx in distal_indices.items()} + + +def decode_auto_data_type(data: bytes) -> AutoDataTypeInfo: + """Decode auto-data-type register (1 byte) into parsed flags.""" + if len(data) != 1: + raise ValueError(f"Auto-data-type register must be exactly 1 byte, got {len(data)}") + val = data[0] + return { + "raw": f"{val:08b}", + "resultant": bool(val & AUTO_DATA_RESULTANT), + "taxels": bool(val & AUTO_DATA_TAXELS), + } + + +def encode_auto_data_type(resultant: bool, taxels: bool) -> bytes: + """Encode auto-data-type register value.""" + val = (AUTO_DATA_RESULTANT if resultant else 0) | (AUTO_DATA_TAXELS if taxels else 0) + return bytes([val]) + + +# ========================================================================= +# Wire-format encoders — fake-hardware fixtures only +# +# Production code never encodes resultant or taxel wire bytes (the sensor +# board is the only encoder on the real bus). These exist so the mock +# client and decoder unit tests can round-trip through the real decoders, +# exercising the wire-format logic instead of bypassing it. Do NOT import +# from runtime code paths. +# ========================================================================= + +def _pack_resultant_for_mock(force: ForceVector) -> bytes: + """Encode one [fx, fy, fz] vector into a 6-byte module slot. + + Each axis is packed as low_byte + sign-extended high byte (0xFF when + low byte > 127, else 0x00) for fx, fy, AND fz — matching the firmware. + """ + def _pack_axis(value_n: float) -> bytes: + lo = round(value_n / RESOLUTION_N_PER_LSB) & 0xFF + hi = 0xFF if lo > 127 else 0x00 + return bytes([lo, hi]) + + return _pack_axis(force[0]) + _pack_axis(force[1]) + _pack_axis(force[2]) + + +def _pack_taxel_for_mock(force: ForceVector) -> bytes: + """Encode one [fx, fy, fz] taxel vector into 3 contiguous bytes.""" + return bytes([ + round(force[0] / RESOLUTION_N_PER_LSB) & 0xFF, + round(force[1] / RESOLUTION_N_PER_LSB) & 0xFF, + round(force[2] / RESOLUTION_N_PER_LSB) & 0xFF, + ]) + + +def encode_resultant_auto_for_mock( + forces: ResultantForces, + active_sensors: list[str], +) -> bytes: + return b"".join(_pack_resultant_for_mock(forces[f]) for f in active_sensors) + + +def encode_taxels_auto_for_mock( + taxels: TaxelForces, + active_sensors: list[str], +) -> bytes: + return b"".join(_pack_taxel_for_mock(t) for f in active_sensors for t in taxels[f]) + + +def encode_combined_auto_for_mock( + forces: ResultantForces, + taxels: TaxelForces, + active_sensors: list[str], +) -> bytes: + """Build interleaved [resultant_i, taxels_i, resultant_{i+1}, ...] payload.""" + out = bytearray() + for f in active_sensors: + out += _pack_resultant_for_mock(forces[f]) + for t in taxels[f]: + out += _pack_taxel_for_mock(t) + return bytes(out) diff --git a/orca_core/hardware/sensing/types.py b/orca_core/hardware/sensing/types.py new file mode 100644 index 00000000..f709aec6 --- /dev/null +++ b/orca_core/hardware/sensing/types.py @@ -0,0 +1,74 @@ +"""Typed containers for tactile sensor readings.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from orca_core.hardware.sensing.constants import FingerName + + +@dataclass(frozen=True) +class ResultantReading: + """Resultant force per finger from a single auto-stream frame. + + Supports dict-style access: ``reading["thumb"]`` returns ``[fx, fy, fz]``. + """ + + forces: dict[str, list[float]] + timestamp: float | None = None + + def __getitem__(self, finger: FingerName) -> list[float]: + return self.forces[finger] + + def __contains__(self, finger: FingerName) -> bool: + return finger in self.forces + + @property + def fingers(self) -> list[str]: + return list(self.forces.keys()) + + def as_array(self) -> np.ndarray: + """Return an ``(n_fingers, 3)`` array, rows in finger-name order.""" + return np.array([self.forces[f] for f in sorted(self.forces)]) + + +@dataclass(frozen=True) +class TaxelReading: + """Per-taxel forces from a single auto-stream frame. + + Supports dict-style access: ``reading["thumb"]`` returns + ``[[fx, fy, fz], ...]`` for every taxel on that finger. + """ + + taxels: dict[str, list[list[float]]] + timestamp: float | None = None + + def __getitem__(self, finger: FingerName) -> list[list[float]]: + return self.taxels[finger] + + def __contains__(self, finger: FingerName) -> bool: + return finger in self.taxels + + @property + def fingers(self) -> list[str]: + return list(self.taxels.keys()) + + def as_array(self, finger: FingerName) -> np.ndarray: + """Return an ``(n_taxels, 3)`` array for *finger*.""" + return np.array(self.taxels[finger]) + + +@dataclass(frozen=True) +class TactileReading: + """Atomic snapshot of resultant + per-taxel forces from a single frame. + + Either field may be ``None`` if the matching stream mode is disabled. + Use this when you need forces and taxels guaranteed to come from the + same frame (one lock acquisition, one timestamp). + """ + + forces: ResultantReading | None + taxels: TaxelReading | None + timestamp: float | None = None diff --git a/orca_core/hardware/tactile_client.py b/orca_core/hardware/tactile_client.py new file mode 100644 index 00000000..4df8708e --- /dev/null +++ b/orca_core/hardware/tactile_client.py @@ -0,0 +1,775 @@ +# ============================================================================== +# Copyright (c) 2025 ORCA Dexterity, Inc. All rights reserved. +# +# This file is part of ORCA Dexterity and is licensed under the MIT License. +# You may use, copy, modify, and distribute this file under the terms of the MIT License. +# See the LICENSE file at the root of this repository for full license information. +# ============================================================================== +from __future__ import annotations + +import dataclasses +from dataclasses import dataclass, field +import serial +import threading +import time +import logging + +from orca_core.hardware.sensing.constants import ( + FINGER_NAMES, + DEFAULT_SENSOR_PORT, + DEFAULT_SENSOR_BAUDRATE, + DEFAULT_FINGER_TO_SENSOR_ID, + PROTOCOL_HEADER_RESPONSE, + PROTOCOL_HEADER_AUTO, + RESPONSE_META_SIZE, + AUTO_FRAME_META_SIZE, + ADDR_CONNECTED_SENSORS_START, + ADDR_CONNECTED_SENSORS_LENGTH, + ADDR_NUM_TAXELS_START, + ADDR_NUM_TAXELS_LENGTH, + ADDR_RESULTANT_FORCE_START, + RESULTANT_BLOCK_SIZE, + ADDR_AUTO_DATA_TYPE, + ADDR_AUTO_ENABLE, + REGISTER_ENABLE, + REGISTER_DISABLE, +) +from orca_core.hardware.sensing.protocol import ( + validate_auto_frame_lrc, + build_read_request, + build_write_request, + parse_read_response, + parse_write_response, + extract_write_response_data_length, + read_response_body_size, + extract_auto_frame_eff_len, + unpack_auto_payload, + compute_expected_payload_size, + compute_distal_module_index, + decode_resultant_auto, + decode_taxels_auto, + decode_combined_auto, + decode_resultant_register, + decode_connected_sensors, + decode_num_taxels, + decode_auto_data_type, + encode_auto_data_type, +) + +logger = logging.getLogger(__name__) + + +class NoSensorsAvailableError(Exception): + pass + + +class FrameError(Exception): + """Recoverable frame-level error in auto-stream acquisition. + + Raised by _acquire_frame when a single frame is bad (LRC failure, + parse error, size mismatch). The auto-reader loop increments error + counters and continues to the next frame. + """ + def __init__(self, message: str, bad_lrc: bool = False): + super().__init__(message) + self.bad_lrc = bad_lrc + + +@dataclass +class AutoStreamStats: + """Diagnostic counters for the auto-stream reader loop.""" + frames_ok: int = 0 + frames_bad_checksum: int = 0 + parse_errors: int = 0 + resyncs: int = 0 + last_error_code: int = 0 # most recent sensor-reported error code (0 = no error) + + +@dataclass +class TactileSensorConfiguration: + """Snapshot of connected sensors and their properties. + + Captured at stream start. The Paxini PX-6AX GEN3 firmware enumerates + sensors at power-on and does not report mid-stream changes, so this + snapshot is treated as immutable for the duration of a stream. + """ + connected: dict[str, bool] = field(default_factory=dict) # {finger: is_connected} + num_taxels: dict[str, int] = field(default_factory=dict) # {finger: taxel_count} + module_indices: dict[str, int] = field(default_factory=dict) # {finger: module_idx} + finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID)) + + @property + def active_sensors(self) -> list[str]: + """List of currently connected sensors sorted by hardware slot order. + + Auto-stream data arrives in slot order, so this must match. + Protocol decoders in protocol.py require this ordering. + """ + active = [f for f in FINGER_NAMES if self.connected.get(f, False)] + active.sort(key=lambda f: self.finger_to_sensor_id.get(f, FINGER_NAMES.index(f))) + return active + + @property + def num_active_sensors(self) -> int: + return len(self.active_sensors) + + def __str__(self) -> str: + active = ", ".join(self.active_sensors) if self.active_sensors else "none" + return f"SensorConfig({self.num_active_sensors} active: {active})" + + +class TactileClient: + """Client for communicating with ORCA Tactile Sensors""" + + def __init__(self, + port: str = DEFAULT_SENSOR_PORT, + baudrate: int = DEFAULT_SENSOR_BAUDRATE, + finger_to_sensor_id: dict[str, int] | None = None): + + self.port = port + self.baudrate = baudrate + self._connected = False + self._serial_connection: serial.Serial | None = None + + # Finger-to-sensor-id mapping (configurable wiring) + if finger_to_sensor_id is None: + self._finger_to_sensor_id = dict(DEFAULT_FINGER_TO_SENSOR_ID) + else: + expected_fingers = set(FINGER_NAMES) + if set(finger_to_sensor_id.keys()) != expected_fingers: + raise ValueError( + f"finger_to_sensor_id must contain exactly {FINGER_NAMES}, " + f"got {sorted(finger_to_sensor_id.keys())}" + ) + ids = sorted(finger_to_sensor_id.values()) + if ids != [0, 1, 2, 3, 4]: + raise ValueError( + f"finger_to_sensor_id values must be 0-4 with no duplicates, " + f"got {sorted(finger_to_sensor_id.values())}" + ) + self._finger_to_sensor_id = dict(finger_to_sensor_id) + self._sensor_id_to_finger = {v: k for k, v in self._finger_to_sensor_id.items()} + + self._tactile_config: TactileSensorConfiguration | None = None + + self._auto_thread: threading.Thread | None = None + self._auto_running = threading.Event() + self._auto_lock = threading.Lock() + self._auto_latest = None + self._auto_latest_taxels = None + self._auto_latest_ts = None + self._auto_stats = AutoStreamStats() + self._auto_mode_resultant = True + self._auto_mode_taxels = False + self._last_frame_debug_print: float = 0.0 + + # {finger: [[fx, fy, fz], ...], ...} per-taxel zeroing offsets. + self._taxel_offsets: dict | None = None + # {finger: [fx, fy, fz], ...} sum of taxel offsets per finger. + self._resultant_offsets: dict | None = None + + @property + def is_connected(self) -> bool: + return self._connected + + def connect(self): + """Open the serial link and capture the initial sensor configuration.""" + if self.is_connected: + return + + try: + self._serial_connection = serial.Serial( + port=self.port, + baudrate=self.baudrate, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + timeout=1.0 + ) + self._connected = True + logger.info(f"Connected to sensor at {self.port}") + + # Initial config is best-effort; start_auto_stream will read it again. + try: + self._tactile_config = self._get_configuration() + logger.info(f"Initial configuration: {self._tactile_config}") + except IOError as e: + logger.warning(f"Failed to get initial configuration: {e}") + + except (serial.SerialException, OSError) as e: + raise ConnectionError(f"Failed to connect to sensor at {self.port}: {e}") from e + + def disconnect(self): + if not self.is_connected: + return + if self._serial_connection and self._serial_connection.is_open: + self._serial_connection.close() + self._connected = False + + def _read_register(self, address: int, count: int = 1, response_timeout_s: float = 0.5) -> bytes: + """Send a read request and return the data bytes from the response. + + Tolerates auto-stream mode: AA56 frames arriving before the AA55 + response are skipped instead of treated as the response. + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + request = build_read_request(address, count) + + # Stale bytes in the input buffer would otherwise be treated as the response. + if not self._is_streaming(): + self._serial_connection.reset_input_buffer() + + self._serial_connection.write(request) + + deadline = time.time() + response_timeout_s + while True: + remaining = deadline - time.time() + if remaining <= 0: + raise TimeoutError("Timed out waiting for read response (AA55).") + + hdr = self._read_header_resync(timeout_s=remaining) + if hdr == PROTOCOL_HEADER_AUTO: + self._skip_auto_frame() + continue + break + + body = self._read_exact(read_response_body_size(count)) + return parse_read_response(hdr + body) + + def _skip_auto_frame(self) -> None: + """Discard one auto-stream frame after the AA56 header has been consumed.""" + meta = self._read_exact(AUTO_FRAME_META_SIZE) + eff_len = extract_auto_frame_eff_len(meta) + _ = self._read_exact(eff_len + 1) # payload + LRC + + def _read_header_resync(self, timeout_s: float) -> bytes: + """Slide a 2-byte window until AA55 (response) or AA56 (auto) is found. + + Needed because the bus may begin mid-frame and AA56 frames can interleave + with AA55 responses at any time. Returns the 2-byte header. + """ + deadline = time.time() + timeout_s + b1 = b"" + while time.time() < deadline: + b = self._serial_connection.read(1) + if not b: + continue + + if not b1: + b1 = b + continue + + hdr = b1 + b + if hdr == PROTOCOL_HEADER_RESPONSE or hdr == PROTOCOL_HEADER_AUTO: + return hdr + + b1 = b + + raise TimeoutError("Timed out waiting for AA55/AA56 header.") + + def _is_streaming(self) -> bool: + return self._auto_running.is_set() + + def _write_register( + self, + address: int, + data: bytes, + response_timeout_s: float = 0.5, + ) -> None: + """Send a write request and validate the status byte in the response. + + Tolerates auto-stream mode the same way as ``_read_register``. + Per Paxini manual, ``data`` is capped at 10 bytes. + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + request = build_write_request(address, data) + + if not self._is_streaming(): + self._serial_connection.reset_input_buffer() + + self._serial_connection.write(request) + + deadline = time.time() + response_timeout_s + while True: + remaining = deadline - time.time() + if remaining <= 0: + raise TimeoutError("Timed out waiting for write response (AA55).") + + hdr = self._read_header_resync(timeout_s=remaining) + if hdr == PROTOCOL_HEADER_AUTO: + self._skip_auto_frame() + continue + break + + meta = self._read_exact(RESPONSE_META_SIZE) + data_len = extract_write_response_data_length(meta) + rest = self._read_exact(data_len + 1) # payload + LRC + parse_write_response(hdr + meta + rest) + + + + def read_connected_sensors(self) -> dict[str, bool]: + """Return ``{finger: is_connected}`` from the connected-sensors register.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + data = self._read_register(ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH) + return decode_connected_sensors(data, self._sensor_id_to_finger) + + def read_num_taxels(self) -> dict[str, int]: + """Return ``{finger: taxel_count}`` from the taxel-count register block.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + data = self._read_register(ADDR_NUM_TAXELS_START, ADDR_NUM_TAXELS_LENGTH) + return decode_num_taxels(data, self._sensor_id_to_finger) + + def read_auto_data_type(self) -> dict: + """Return the decoded auto-stream data-type register.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + data = self._read_register(ADDR_AUTO_DATA_TYPE, 1) + return decode_auto_data_type(data) + + def _read_raw_resultant(self) -> dict[str, list[float]]: + """Read raw resultant forces from hardware. Overridden by MockTactileClient.""" + if self._tactile_config is None: + try: + self._tactile_config = self._get_configuration() + except IOError as e: + logger.error(f"Failed to get configuration: {e}") + # Fall back to assuming all 5 sensors are connected. + data = self._read_register(ADDR_RESULTANT_FORCE_START, RESULTANT_BLOCK_SIZE) + module_indices = {f: compute_distal_module_index(self._finger_to_sensor_id[f]) for f in FINGER_NAMES} + return decode_resultant_register(data, list(FINGER_NAMES), module_indices) + + data = self._read_register(ADDR_RESULTANT_FORCE_START, RESULTANT_BLOCK_SIZE) + return decode_resultant_register( + data, self._tactile_config.active_sensors, self._tactile_config.module_indices, + ) + + def read_resultant_force(self) -> dict[str, list[float]]: + """Read resultant force from all connected fingertip sensors, applying offsets.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + result = self._read_raw_resultant() + if self._resultant_offsets: + self._apply_resultant_offsets(result) + return result + + def get_tactile_configuration(self) -> TactileSensorConfiguration | None: + """Return the cached sensor configuration, or ``None`` if never read.""" + return self._tactile_config + + def _get_configuration(self) -> TactileSensorConfiguration: + """Read the current connected-sensors and taxel-count registers from hardware.""" + try: + connected = self.read_connected_sensors() + num_taxels = self.read_num_taxels() + + module_indices = {} + for finger in FINGER_NAMES: + if connected.get(finger, False): + sensor_id = self._finger_to_sensor_id[finger] + module_indices[finger] = compute_distal_module_index(sensor_id) + + config = TactileSensorConfiguration( + connected=connected, + num_taxels=num_taxels, + module_indices=module_indices, + finger_to_sensor_id=dict(self._finger_to_sensor_id), + ) + + logger.info(f"Configuration captured: {config}") + return config + + except (IOError, FrameError) as e: + logger.error(f"Failed to get sensor configuration: {e}") + raise IOError(f"Failed to read sensor configuration: {e}") from e + + def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: + """Configure which data types are included in auto-stream frames.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + self._write_register(ADDR_AUTO_DATA_TYPE, encode_auto_data_type(resultant, taxels)) + + + def enable_auto_data_transmission(self) -> None: + if not self.is_connected: + raise OSError("Must call connect() first.") + self._write_register(ADDR_AUTO_ENABLE, REGISTER_ENABLE) + + def disable_auto_data_transmission(self) -> None: + if not self.is_connected: + raise OSError("Must call connect() first.") + self._write_register(ADDR_AUTO_ENABLE, REGISTER_DISABLE) + + + def get_auto_latest(self): + """Return ``(forces, timestamp)`` for the most recent resultant frame, or ``(None, None)``.""" + with self._auto_lock: + return self._auto_latest, self._auto_latest_ts + + def get_auto_latest_taxels(self): + """Return ``(taxels, timestamp)`` for the most recent taxel frame, or ``(None, None)``.""" + with self._auto_lock: + return self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_latest_all(self): + """Return ``(forces, taxels, timestamp)`` from a single locked read. + + Use this in combined mode so forces and taxels share one timestamp. + """ + with self._auto_lock: + return self._auto_latest, self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_stats(self): + """Return a snapshot copy of ``AutoStreamStats`` for the current loop.""" + with self._auto_lock: + return dataclasses.replace(self._auto_stats) + + def set_taxel_offsets(self, offsets: dict) -> None: + """Store per-taxel zeroing offsets and derive matching resultant offsets.""" + self._taxel_offsets = offsets + self._resultant_offsets = {} + for finger, taxel_list in offsets.items(): + sum_fx = sum(t[0] for t in taxel_list) + sum_fy = sum(t[1] for t in taxel_list) + sum_fz = sum(t[2] for t in taxel_list) + self._resultant_offsets[finger] = [sum_fx, sum_fy, sum_fz] + + def clear_taxel_offsets(self) -> None: + """Clear all zeroing offsets.""" + self._taxel_offsets = None + self._resultant_offsets = None + + def capture_taxel_offsets(self, num_samples: int = 100) -> dict: + """Average ``num_samples`` taxel frames and apply the result as zeroing offsets. + + Requires an active auto-stream with taxels enabled. Existing offsets + are temporarily cleared so the average reflects raw readings. + """ + if not self._is_streaming() or not self._auto_mode_taxels: + raise RuntimeError("Auto-stream with taxels must be active to capture offsets") + + # Clear any current offsets so we average raw frames, not offset-corrected ones. + prev_taxel = self._taxel_offsets + prev_resultant = self._resultant_offsets + self._taxel_offsets = None + self._resultant_offsets = None + + # Let the reader thread emit at least one new frame past the offset clear. + time.sleep(0.01) + + succeeded = False + try: + frames = [] + last_ts = None + while len(frames) < num_samples: + taxels, ts = self.get_auto_latest_taxels() + if taxels is not None and ts != last_ts: + frames.append(taxels) + last_ts = ts + time.sleep(0.002) + + fingers = list(frames[0].keys()) + offsets = {} + for finger in fingers: + num_taxels = len(frames[0][finger]) + avg = [] + for t_idx in range(num_taxels): + sum_fx = sum(f[finger][t_idx][0] for f in frames) + sum_fy = sum(f[finger][t_idx][1] for f in frames) + sum_fz = sum(f[finger][t_idx][2] for f in frames) + avg.append([ + round(sum_fx / num_samples, 2), + round(sum_fy / num_samples, 2), + round(sum_fz / num_samples, 2), + ]) + offsets[finger] = avg + + self.set_taxel_offsets(offsets) + succeeded = True + return offsets + finally: + if not succeeded: + self._taxel_offsets = prev_taxel + self._resultant_offsets = prev_resultant + + def _apply_taxel_offsets(self, taxels: dict) -> None: + """Subtract per-taxel offsets in-place. Clamps fz to >= 0.""" + for finger, taxel_list in taxels.items(): + finger_offsets = self._taxel_offsets.get(finger) + if not finger_offsets: + continue + for i, taxel in enumerate(taxel_list): + if i >= len(finger_offsets): + break + off = finger_offsets[i] + taxel[0] = round(taxel[0] - off[0], 1) + taxel[1] = round(taxel[1] - off[1], 1) + taxel[2] = round(max(0, taxel[2] - off[2]), 1) + + def _apply_resultant_offsets(self, forces: dict) -> None: + """Subtract resultant offsets in-place. Clamps fz to >= 0.""" + for finger, fvec in forces.items(): + off = self._resultant_offsets.get(finger) + if not off: + continue + fvec[0] = round(fvec[0] - off[0], 1) + fvec[1] = round(fvec[1] - off[1], 1) + fvec[2] = round(max(0, fvec[2] - off[2]), 1) + + def _on_frame_stored(self) -> None: + """Hook for subclasses to signal frame availability (e.g. for tests).""" + + def _apply_stream_offsets( + self, + parsed_resultant: dict | None, + parsed_taxels: dict | None, + ) -> None: + """Apply zeroing offsets to parsed auto-stream data in-place.""" + if self._taxel_offsets and parsed_taxels: + self._apply_taxel_offsets(parsed_taxels) + if self._resultant_offsets and parsed_resultant: + self._apply_resultant_offsets(parsed_resultant) + + def _read_exact(self, n: int) -> bytes: + """Read exactly n bytes, raising IOError if the serial 1.0s timeout fires.""" + out = bytearray() + while len(out) < n: + chunk = self._serial_connection.read(n - len(out)) + if chunk is None or len(chunk) == 0: + raise IOError(f"Serial read timeout after reading {len(out)}/{n} bytes") + out.extend(chunk) + return bytes(out) + + def _resync_to_auto_header(self) -> None: + """Slide a 2-byte window over the stream until 0xAA 0x56 is found. + + Exits and raises IOError when ``_auto_running`` is cleared, so a + stop request unblocks this loop instead of hanging on the next read. + """ + b1 = self._read_exact(1) + while self._auto_running.is_set(): + b2 = self._read_exact(1) + if b1 + b2 == PROTOCOL_HEADER_AUTO: + return + b1 = b2 + + raise IOError("Auto stream stopped during resync") + + def _get_expected_payload_size(self, config: TactileSensorConfiguration) -> int: + """Get expected payload size based on current streaming mode.""" + return compute_expected_payload_size( + self._auto_mode_resultant, + self._auto_mode_taxels, + config.active_sensors, + config.num_taxels, + ) + + def _acquire_frame( + self, + parse_resultant: bool, + parse_taxels: bool, + ) -> tuple[dict | None, dict | None]: + """Read one frame from serial, validate LRC, decode according to mode. + + Overridden by ``MockTactileClient`` to swap the data source while + keeping the surrounding stats, offset, and lifecycle logic. + """ + self._resync_to_auto_header() + + meta = self._read_exact(AUTO_FRAME_META_SIZE) + eff_len = extract_auto_frame_eff_len(meta) + payload = self._read_exact(eff_len) + lrc = self._read_exact(1)[0] + + now = time.time() + if now - self._last_frame_debug_print > 1.0: + config_str = str(self._tactile_config) if self._tactile_config else "no config" + logger.debug(f"[auto] eff_len={eff_len}, config={config_str}") + self._last_frame_debug_print = now + + if not validate_auto_frame_lrc(meta, payload, lrc): + raise FrameError("LRC mismatch", bad_lrc=True) + + err_code, valid = unpack_auto_payload(payload) + + with self._auto_lock: + self._auto_stats.last_error_code = err_code + + if not self._tactile_config: + raise FrameError("No sensor configuration available") + + expected_size = self._get_expected_payload_size(self._tactile_config) + if len(valid) != expected_size or expected_size == 0: + raise FrameError( + f"Unexpected payload: {len(valid)} bytes, expected {expected_size}" + ) + + cfg = self._tactile_config + if parse_resultant and parse_taxels: + parsed_resultant, parsed_taxels = decode_combined_auto( + valid, cfg.active_sensors, cfg.num_taxels, + ) + elif parse_resultant: + parsed_resultant = decode_resultant_auto(valid, cfg.active_sensors) + parsed_taxels = None + elif parse_taxels: + parsed_resultant = None + parsed_taxels = decode_taxels_auto( + valid, cfg.active_sensors, cfg.num_taxels, + ) + else: + parsed_resultant = None + parsed_taxels = None + + return parsed_resultant, parsed_taxels + + def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): + """Background thread: acquire → offset → store → repeat. + """ + while self._auto_running.is_set(): + try: + parsed_resultant, parsed_taxels = self._acquire_frame( + parse_resultant, parse_taxels + ) + + self._apply_stream_offsets(parsed_resultant, parsed_taxels) + + with self._auto_lock: + if parse_resultant and parsed_resultant is not None: + self._auto_latest = parsed_resultant + if parse_taxels and parsed_taxels is not None: + self._auto_latest_taxels = parsed_taxels + self._auto_latest_ts = time.time() + self._auto_stats.frames_ok += 1 + self._on_frame_stored() + + except FrameError as e: + with self._auto_lock: + if e.bad_lrc: + self._auto_stats.frames_bad_checksum += 1 + else: + self._auto_stats.parse_errors += 1 + + except IOError as e: + if "Auto stream stopped" in str(e): + logger.info("Auto stream stopped") + break + logger.warning(f"IO error in auto reader: {e}") + with self._auto_lock: + self._auto_stats.resyncs += 1 + time.sleep(0.01) + + except Exception: + logger.exception("Unexpected error in auto reader, stopping stream") + self._auto_running.clear() + break + + logger.info("Auto reader loop exited") + + def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1): + """Start the ~1 kHz auto-stream and the background reader thread. + + Read the latest frame via ``get_auto_latest`` / ``get_auto_latest_taxels`` / + ``get_auto_latest_all``. ``min_sensors`` gates startup: if fewer sensors + enumerate at power-on, ``NoSensorsAvailableError`` is raised. + + Sensor presence is fixed at stream start. The Paxini PX-6AX GEN3 board + enumerates at power-on and does not report mid-stream disconnects — + an unplugged slot keeps emitting its last bytes until the board is + power-cycled. + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + if not resultant and not taxels: + raise ValueError("At least one of resultant or taxels must be enabled") + + self.stop_auto_stream() + + self._auto_mode_resultant = resultant + self._auto_mode_taxels = taxels + + try: + self._tactile_config = self._get_configuration() + except IOError as e: + raise OSError(f"Failed to get sensor configuration: {e}") from e + + if self._tactile_config.num_active_sensors < min_sensors: + raise NoSensorsAvailableError( + f"Only {self._tactile_config.num_active_sensors} sensor(s) available, " + f"need at least {min_sensors}" + ) + + expected_size = self._get_expected_payload_size(self._tactile_config) + mode_str = [] + if resultant: + mode_str.append("resultant") + if taxels: + mode_str.append("taxels") + logger.info( + f"Starting auto-stream with {self._tactile_config}, " + f"mode={'+'.join(mode_str)}, expected_payload={expected_size} bytes" + ) + + # Disable in case the sensor was left in auto mode from a prior run; + # _write_register tolerates incoming AA56 frames if it is still streaming. + try: + self.disable_auto_data_transmission() + except IOError: + pass + + self.set_auto_data_type(resultant=resultant, taxels=taxels) + + if self._serial_connection is not None: + self._serial_connection.reset_input_buffer() + + self.enable_auto_data_transmission() + + self._auto_running.set() + self._auto_thread = threading.Thread( + target=self._auto_reader_loop, + args=(resultant, taxels), + daemon=True, + ) + self._auto_thread.start() + + + def stop_auto_stream(self): + """Stop auto-stream mode and clean up the background thread. + + Safe to call multiple times and when the sensor is already disconnected. + """ + self._auto_running.clear() + + if self._auto_thread is not None: + self._auto_thread.join(timeout=1.0) + self._auto_thread = None + + if self.is_connected: + try: + self.disable_auto_data_transmission() + except IOError: + pass + + with self._auto_lock: + self._auto_latest = None + self._auto_latest_taxels = None + self._auto_latest_ts = None + + def __enter__(self): + if not self.is_connected: + self.connect() + return self + + def __exit__(self, *args): + self.disconnect() diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 4da34d6f..26b1eb4b 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -19,15 +19,18 @@ from .base_hand import BaseHand from .calibration import CalibrationResult -from .hand_config import OrcaHandConfig +from .hand_config import OrcaHandConfig, OrcaHandTouchConfig from .hardware.dynamixel_client import DynamixelClient from .hardware.feetech_client import FeetechClient +from .hardware.mock_tactile_client import MockTactileClient from .hardware.motor_client import MotorClient +from .hardware.sensing.types import ResultantReading, TactileReading, TaxelReading +from .hardware.tactile_client import TactileClient from .utils.utils import ( auto_detect_port, find_single_usb_serial_port, get_and_choose_port, - motor_type_for_port, + read_yaml, update_yaml, ) @@ -1285,6 +1288,164 @@ def stop_task(self): print("No running task to stop.") +class OrcaHandTouch(OrcaHand): + """ORCA hand with integrated tactile sensing. + + ``connect()`` opens both the motor bus and the sensor serial link; + ``disconnect()`` tears down both. + """ + + config_cls = OrcaHandTouchConfig + + def __init__( + self, + config_path: str | None = None, + calibration_path: str | None = None, + model_version: str | None = None, + model_name: str | None = None, + config: OrcaHandTouchConfig | None = None, + ): + super().__init__( + config_path=config_path, + calibration_path=calibration_path, + model_version=model_version, + model_name=model_name, + config=config, + ) + self._tactile_client = None + + def _create_tactile_client(self): + return TactileClient( + port=self.config.sensor_port, + baudrate=self.config.sensor_baudrate, + finger_to_sensor_id=self.config.finger_to_sensor_id, + ) + + def _persist_sensor_port(self, chosen_port: str) -> None: + """Write a new ``sensors.port`` value to ``config.yaml`` while preserving + the rest of the ``sensors`` block (baudrate, finger_to_sensor_id).""" + existing = read_yaml(self.config.config_path) or {} + sensors = dict(existing.get("sensors") or {}) + sensors["port"] = chosen_port + update_yaml(self.config.config_path, "sensors", sensors) + + def _connect_sensor_with_fallback(self) -> tuple[bool, str]: + """Open the sensor serial link, mirroring the motor cascade. + + Tries the configured port first, then USB-VID auto-detection + (``KNOWN_VIDS["tactile_sensor"]``). On a successful auto-detect the + new port is written back to ``config.yaml``. + """ + self._tactile_client = self._create_tactile_client() + try: + self._tactile_client.connect() + return True, f"Sensor connected on {self.config.sensor_port}" + except Exception as e: + print(f"Sensor connection failed on {self.config.sensor_port}: {e}") + self._tactile_client = None + + chosen = auto_detect_port("tactile_sensor") + if chosen and chosen != self.config.sensor_port: + try: + self.config = dataclasses.replace(self.config, sensor_port=chosen) + self._tactile_client = self._create_tactile_client() + self._tactile_client.connect() + self._persist_sensor_port(chosen) + return True, f"Sensor connected on auto-detected {chosen}" + except Exception as e: + print(f"Auto-detected sensor port {chosen} also failed: {e}") + self._tactile_client = None + + return False, ( + "Sensor connection failed: no usable port (set sensors.port in config.yaml " + "or check that the sensor adapter is plugged in)" + ) + + def connect(self) -> tuple[bool, str]: + success, msg = super().connect() + if not success: + return success, msg + + sensor_ok, sensor_msg = self._connect_sensor_with_fallback() + if not sensor_ok: + return False, f"{msg} | {sensor_msg}" + return True, f"{msg} | {sensor_msg}" + + def connect_sensors_only(self) -> tuple[bool, str]: + """Connect only the tactile sensor, skipping the motor bus. + + Useful for sensor bring-up and testing on a hand whose motors are not + powered. After this call, tactile methods work; motor-control methods + will fail because the motor client is not initialised. + """ + return self._connect_sensor_with_fallback() + + def disconnect(self) -> None: + if self._tactile_client is not None and self._tactile_client.is_connected: + try: + self._tactile_client.stop_auto_stream() + except Exception: + pass + self._tactile_client.disconnect() + self._tactile_client = None + super().disconnect() + + def get_tactile_forces(self) -> ResultantReading | None: + """Return the latest resultant ``ResultantReading``, or ``None`` if no frame yet.""" + forces, ts = self._tactile_client.get_auto_latest() + if forces is None: + return None + return ResultantReading(forces=forces, timestamp=ts) + + def get_tactile_taxels(self) -> TaxelReading | None: + """Return the latest per-taxel ``TaxelReading``, or ``None`` if no frame yet.""" + taxels, ts = self._tactile_client.get_auto_latest_taxels() + if taxels is None: + return None + return TaxelReading(taxels=taxels, timestamp=ts) + + def get_tactile_data(self) -> TactileReading | None: + """Return resultant and per-taxel forces from the same frame. + + Single locked snapshot of the auto-stream cache, so ``forces`` and + ``taxels`` are guaranteed to share a timestamp. Either field is + ``None`` if its stream mode is disabled. Returns ``None`` if no + frame has arrived yet. + """ + forces, taxels, ts = self._tactile_client.get_auto_latest_all() + if forces is None and taxels is None: + return None + return TactileReading( + forces=ResultantReading(forces=forces, timestamp=ts) if forces is not None else None, + taxels=TaxelReading(taxels=taxels, timestamp=ts) if taxels is not None else None, + timestamp=ts, + ) + + def start_tactile_stream( + self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1 + ) -> None: + self._tactile_client.start_auto_stream( + resultant=resultant, taxels=taxels, min_sensors=min_sensors, + ) + + def stop_tactile_stream(self) -> None: + self._tactile_client.stop_auto_stream() + + def zero_tactile_sensors(self, num_samples: int = 100) -> dict: + """Capture current readings as zero baseline and return offsets.""" + return self._tactile_client.capture_taxel_offsets(num_samples=num_samples) + + def clear_tactile_zero(self) -> None: + self._tactile_client.clear_taxel_offsets() + + def get_tactile_configuration(self): + return self._tactile_client.get_tactile_configuration() + + def get_tactile_stats(self): + """Return ``AutoStreamStats`` for the running auto-stream.""" + return self._tactile_client.get_auto_stats() + + class MockOrcaHand(OrcaHand): """Drop-in :class:`OrcaHand` backed by an in-memory mock motor client, for testing and prototyping. @@ -1308,4 +1469,24 @@ def _create_motor_client( ) -> MotorClient: from .hardware.mock_dynamixel_client import MockDynamixelClient - return MockDynamixelClient(self.config.motor_ids, port, baudrate) + return MockDynamixelClient( + self.config.motor_ids, self.config.port, self.config.baudrate + ) + + +class MockOrcaHandTouch(OrcaHandTouch): + """Drop-in :class:`OrcaHandTouch` with in-memory mock motor + sensor clients (no serial I/O).""" + + def _create_motor_client(self) -> MotorClient: + from .hardware.mock_dynamixel_client import MockDynamixelClient + + return MockDynamixelClient( + self.config.motor_ids, self.config.port, self.config.baudrate + ) + + def _create_tactile_client(self): + return MockTactileClient( + port="mock", + baudrate=self.config.sensor_baudrate, + finger_to_sensor_id=self.config.finger_to_sensor_id, + ) diff --git a/orca_core/models/v2/orcahand-touch/config.yaml b/orca_core/models/v2/orcahand-touch/config.yaml new file mode 100644 index 00000000..86bfab2e --- /dev/null +++ b/orca_core/models/v2/orcahand-touch/config.yaml @@ -0,0 +1,211 @@ +port: /dev/ttyACM2 +version: 0.2.1 +baudrate: 1000000 +max_current: 400 +type: right +control_mode: current_based_position +motor_ids: +- 1 +- 2 +- 3 +- 4 +- 5 +- 6 +- 7 +- 8 +- 9 +- 10 +- 11 +- 12 +- 13 +- 14 +- 15 +- 16 +- 17 +joint_ids: +- wrist +- thumb_cmc +- thumb_abd +- thumb_mcp +- thumb_dip +- index_abd +- index_mcp +- index_pip +- middle_abd +- middle_mcp +- middle_pip +- ring_abd +- ring_mcp +- ring_pip +- pinky_abd +- pinky_mcp +- pinky_pip +joint_to_motor_map: + thumb_cmc: 17 + thumb_abd: 14 + thumb_mcp: 15 + thumb_dip: 16 + index_abd: 4 + index_mcp: 3 + index_pip: 2 + middle_abd: 5 + middle_mcp: 10 + middle_pip: 9 + ring_abd: 6 + ring_mcp: -7 + ring_pip: 8 + pinky_abd: -13 + pinky_mcp: 12 + pinky_pip: -11 + wrist: -1 +joint_roms: + wrist: + - -65 + - 35 + thumb_cmc: + - -45 + - 33 + thumb_abd: + - -18 + - 55 + thumb_mcp: + - -25 + - 100 + thumb_dip: + - -15 + - 107 + index_abd: + - -30 + - 25 + index_mcp: + - -25 + - 100 + index_pip: + - -15 + - 107 + middle_abd: + - -27 + - 27 + middle_mcp: + - -25 + - 100 + middle_pip: + - -15 + - 107 + ring_abd: + - -27 + - 27 + ring_mcp: + - -25 + - 100 + ring_pip: + - -15 + - 107 + pinky_abd: + - -30 + - 30 + pinky_mcp: + - -25 + - 100 + pinky_pip: + - -15 + - 107 +neutral_position: + thumb_cmc: 0 + thumb_abd: 50 + thumb_mcp: 33 + thumb_dip: 18 + index_abd: -14 + index_mcp: 2 + index_pip: 6 + middle_abd: -4 + middle_mcp: 2 + middle_pip: 4 + ring_abd: 10 + ring_mcp: -2 + ring_pip: 8 + pinky_abd: 22 + pinky_mcp: -4 + pinky_pip: -2 + wrist: -20 +calib_current: 300 +calib_step_size: 0.15 +calib_step_period: 0.0001 +calib_num_stable: 10 +calib_threshold: 0.01 +calib_sequence: +- step: 1 + joints: + thumb_cmc: flex +- step: 2 + joints: + thumb_cmc: extend +- step: 3 + joints: + thumb_abd: flex +- step: 4 + joints: + thumb_abd: extend +- step: 5 + joints: + thumb_mcp: flex +- step: 6 + joints: + thumb_mcp: extend +- step: 7 + joints: + thumb_dip: flex +- step: 8 + joints: + thumb_dip: extend +- step: 9 + joints: + index_abd: flex + middle_abd: flex + ring_abd: flex + pinky_abd: flex +- step: 10 + joints: + index_abd: extend + middle_abd: extend + ring_abd: extend + pinky_abd: extend +- step: 11 + joints: + index_mcp: flex + middle_mcp: flex + ring_mcp: flex + pinky_mcp: flex +- step: 12 + joints: + index_mcp: extend + middle_mcp: extend + ring_mcp: extend + pinky_mcp: extend +- step: 13 + joints: + index_pip: flex + middle_pip: flex + ring_pip: flex + pinky_pip: flex +- step: 14 + joints: + index_pip: extend + middle_pip: extend + ring_pip: extend + pinky_pip: extend +- step: 15 + joints: + wrist: flex +- step: 16 + joints: + wrist: extend +sensors: + port: /dev/ttyACM0 + baudrate: 921600 + finger_to_sensor_id: + thumb: 1 + index: 3 + middle: 0 + ring: 2 + pinky: 4 diff --git a/pyproject.toml b/pyproject.toml index 487b8a69..2b4ffc4f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -10,6 +10,7 @@ readme = "README.md" requires-python = ">=3.10" dependencies = [ "dynamixel-sdk (>=3.7.31,<4.0.0)", + "pyserial (>=3.5,<4.0)", "pyyaml (>=6.0.2,<7.0.0)", "fastapi (>=0.115.12,<0.116.0)", "uvicorn (>=0.34.2,<0.35.0)", diff --git a/scripts/test_sensors.py b/scripts/test_sensors.py new file mode 100644 index 00000000..a480af55 --- /dev/null +++ b/scripts/test_sensors.py @@ -0,0 +1,505 @@ +#!/usr/bin/env python +"""Tactile sensor health check for an assembled OrcaHandTouch. + +Walks through 6 interactive phases that verify all 5 tactile sensors +enumerate, stream at ~1 kHz, respond to finger presses, and zero correctly. +Run on a fully assembled hand with all 5 sensors plugged in. Motors do not +need to be powered or connected — only the sensor adapter. + +For a complementary live visualization (force arrows, taxel heatmap), see +orca_ui: https://github.com/orcahand/orca_ui + +Usage: + uv run python scripts/test_sensors.py orca_core/models/v2/orcahand-touch +""" + +import argparse +import sys +import time + +from orca_core import OrcaHandTouch + +# Thresholds and taxel layouts are intentionally script-local — they only +# drive pass/fail decisions and the ASCII renderer here, not the runtime API. + +FINGERS = ["thumb", "index", "middle", "ring", "pinky"] + +PRESS_THRESHOLD_N = 1.0 +ZERO_TOLERANCE_N = 0.5 +TAXEL_ACTIVE_N = 0.1 # |fz| above this counts as "pressed" in the ASCII grid + +# (rows, cols, positions) per (role, taxel_count). role ∈ {"thumb","finger","pinky"}. +# positions[i] = (row, col) of taxel i in the auto-stream sequence; row=0 is the +# fingertip. Multiple taxels may share a cell — the renderer ORs hot states. +TAXEL_LAYOUTS: dict[tuple[str, int], tuple[int, int, tuple[tuple[int, int], ...]]] = { + ("finger", 87): (18, 19, ( + (1,13), (3,16), (3,16), (1,13), (3,15), (3,9), + (3,12), (1,9), (0,9), (17,17), (17,14), (17,9), + (15,9), (14,9), (12,9), (10,9), (8,9), (7,9), + (5,9), (5,16), (7,16), (8,15), (10,15), (12,15), + (14,15), (15,16), (10,12), (14,12), (15,13), (12,12), + (7,13), (8,12), (5,13), (4,17), (6,17), (8,17), + (10,17), (11,17), (13,17), (15,18), (17,18), (17,18), + (10,17), (14,17), (15,18), (12,17), (6,17), (8,17), + (5,17), (1,5), (3,2), (3,2), (1,5), (3,3), + (3,6), (17,1), (17,4), (5,2), (7,2), (8,3), + (10,3), (12,3), (14,3), (15,2), (10,6), (14,6), + (15,5), (12,6), (7,5), (8,6), (5,5), (4,1), + (6,1), (8,1), (10,1), (11,1), (13,1), (15,0), + (17,0), (17,0), (10,1), (14,1), (15,0), (12,1), + (6,1), (8,1), (5,1), + )), + ("thumb", 51): (11, 21, ( + (10,0), (10,1), (8,0), (8,1), (6,0), (6,1), + (4,1), (10,3), (8,3), (6,3), (4,2), (4,3), + (2,3), (2,3), (10,6), (8,6), (6,6), (3,4), + (4,6), (0,6), (1,7), (2,7), (9,10), (8,10), + (6,10), (4,10), (2,10), (1,10), (0,10), (2,13), + (10,14), (6,14), (4,14), (1,13), (8,14), (0,14), + (3,16), (10,17), (8,17), (6,17), (4,17), (2,17), + (10,19), (10,20), (8,19), (8,20), (6,19), (6,20), + (4,19), (4,18), (2,17), + )), + ("pinky", 51): (11, 17, ( + (10,0), (10,0), (8,0), (8,0), (6,0), (6,0), + (4,1), (10,1), (8,1), (6,1), (5,1), (5,2), + (2,2), (3,2), (10,5), (8,5), (6,4), (3,2), + (4,4), (1,4), (1,4), (2,5), (10,8), (8,8), + (6,8), (4,8), (2,8), (1,8), (0,8), (2,11), + (10,11), (6,12), (4,12), (1,12), (8,11), (1,12), + (3,14), (10,15), (8,15), (6,15), (5,14), (3,14), + (10,16), (10,16), (8,16), (8,16), (6,16), (6,16), + (4,15), (5,15), (2,14), + )), +} + +FINGER_TO_ROLE = {"thumb": "thumb", "index": "finger", "middle": "finger", + "ring": "finger", "pinky": "pinky"} + + +def render_taxel_grid(role: str, num_taxels: int, taxels, threshold_n: float = TAXEL_ACTIVE_N) -> str | None: + """Return multi-line ASCII art of taxel state (X = active, O = inactive, + space = no taxel at that cell), or None if no layout exists for this + (role, taxel_count) combination.""" + layout = TAXEL_LAYOUTS.get((role, num_taxels)) + if layout is None: + return None + rows, cols, positions = layout + grid = [[" "] * cols for _ in range(rows)] + for idx, (r, c) in enumerate(positions): + active = abs(taxels[idx][2]) > threshold_n + if active: + grid[r][c] = "X" + elif grid[r][c] == " ": + grid[r][c] = "O" + return "\n".join(" " + " ".join(row) for row in grid) + + +def banner(n, name): + print(f"\n===== PHASE {n}: {name} =====") + + +def pause(msg): + input(f">>> {msg} (press Enter) ") + + +def wait_for_frame(getter, timeout=2.0): + deadline = time.time() + timeout + while time.time() < deadline: + if getter() is not None: + return + time.sleep(0.005) + raise TimeoutError(f"No auto-stream frame within {timeout}s") + + +def measure_all_peaks(getter, fingers, duration_s=1.5, taxels=False): + """Return dict {finger: peak |fz|} over the duration, for every finger + in `fingers`. Lets us detect wiring mismatches (signal showing up under + the wrong finger name).""" + peaks = {f: 0.0 for f in fingers} + end = time.time() + duration_s + while time.time() < end: + reading = getter() + if reading is not None: + for f in fingers: + if f not in reading: + continue + if taxels: + val = max((abs(t[2]) for t in reading[f]), default=0.0) + else: + val = abs(reading[f][2]) + if val > peaks[f]: + peaks[f] = val + time.sleep(0.002) + return peaks + + +def _redraw_in_place(prev_lines: int, lines: list[str]) -> int: + """Move the cursor up `prev_lines`, clear each line, write the new ones, + and flush. Returns the number of lines just written. Uses ANSI escape + sequences (works on macOS / Linux terminals; Windows cmd not supported).""" + if prev_lines: + sys.stdout.write(f"\033[{prev_lines}F") # cursor up N lines, to col 0 + for ln in lines: + sys.stdout.write("\033[2K" + ln + "\n") # clear line, write, advance + sys.stdout.flush() + return len(lines) + + +def live_press_resultant(hand, target, fingers, duration_s=1.5, fps=20): + """Live in-place display of resultant |fz| during press of `target`. + Polls at ~200 Hz for accurate peak tracking; redraws at `fps`. + Returns dict {finger: peak |fz|} across the full window.""" + peaks = {f: 0.0 for f in fingers} + end = time.time() + duration_s + interval = 1.0 / fps + next_render = time.time() + prev_lines = 0 + while time.time() < end: + reading = hand.get_tactile_forces() + if reading is not None: + for f in fingers: + if f in reading: + v = abs(reading[f][2]) + if v > peaks[f]: + peaks[f] = v + if time.time() >= next_render: + cur = abs(reading[target][2]) if reading and target in reading else 0.0 + line = f" {target}: |fz| now={cur:5.2f} N peak={peaks[target]:5.2f} N" + prev_lines = _redraw_in_place(prev_lines, [line]) + next_render = time.time() + interval + time.sleep(0.005) + return peaks + + +def live_press_taxels(hand, target, role, n_taxels, fingers, duration_s=1.5, fps=12): + """Live in-place display of taxel ASCII grid + peak during press of + `target`. Polls at ~200 Hz, redraws at `fps`. Returns dict + {finger: peak |fz| at hottest taxel} across the full window.""" + peaks = {f: 0.0 for f in fingers} + end = time.time() + duration_s + interval = 1.0 / fps + next_render = time.time() + prev_lines = 0 + no_layout = (f" (no ASCII layout for {role}-{n_taxels}; " + f"standard models are thumb-51, finger-87, pinky-51)") + while time.time() < end: + reading = hand.get_tactile_taxels() + if reading is not None: + for f in fingers: + if f in reading: + v = max((abs(t[2]) for t in reading[f]), default=0.0) + if v > peaks[f]: + peaks[f] = v + if time.time() >= next_render: + if reading is not None and target in reading: + cur_max = max((abs(t[2]) for t in reading[target]), default=0.0) + grid = render_taxel_grid(role, n_taxels, reading[target]) + else: + cur_max = 0.0 + grid = None + header = (f" {target}: |fz| now={cur_max:5.2f} N peak={peaks[target]:5.2f} N" + f" ({n_taxels} taxels, X = >{TAXEL_ACTIVE_N:.1f}N)") + lines = [header] + if grid is None: + lines.append(no_layout) + else: + lines.extend(grid.split("\n")) + prev_lines = _redraw_in_place(prev_lines, lines) + next_render = time.time() + interval + time.sleep(0.005) + return peaks + + +def detect_wiring_mismatch(target_finger, peaks, wiring, threshold=PRESS_THRESHOLD_N): + """If user pressed `target_finger` but the largest signal showed up + under a different finger name, return a helpful suggestion string. + Otherwise return None.""" + target_peak = peaks.get(target_finger, 0.0) + if target_peak >= threshold: + return None + others = {f: p for f, p in peaks.items() if f != target_finger and p >= threshold} + if not others: + return None + other_finger = max(others, key=others.get) + target_slot = wiring.get(target_finger) + other_slot = wiring.get(other_finger) + return ( + f" WIRING MISMATCH: pressed {target_finger.upper()} but force showed under " + f"{other_finger.upper()} ({others[other_finger]:.2f} N vs {target_peak:.2f} N on {target_finger}).\n" + f" → Likely fix: in config.yaml's finger_to_sensor_id (or " + f"hardware/sensing/constants.py), set '{target_finger}' to {other_slot} " + f"(currently {target_slot}). You'll then need to reassign '{other_finger}' too." + ) + + +def phase_1_enumerate(hand): + banner(1, "Connect & enumerate") + cfg = hand.get_tactile_configuration() + print(f" {cfg}") + print(" Per-finger status (canonical order):") + for f in FINGERS: + connected = cfg.connected.get(f, False) + n_taxels = cfg.num_taxels.get(f, 0) + slot = hand.config.finger_to_sensor_id.get(f) + print(f" {f:7s} connected={connected!s:5s} taxels={n_taxels:3d} slot={slot}") + + missing = [f for f in FINGERS if not cfg.connected.get(f, False)] + if missing: + return False, f"missing sensors: {missing} (need all 5)" + return True, "all 5 sensors connected" + + +def prep_zero_baseline(hand): + """Capture and apply per-taxel zero offsets so all subsequent phases + display zero-relative readings. Not a numbered test phase — it's setup + for the press phases. Phase 5 still independently re-tests the zeroing + workflow (re-zero, press detection, clear).""" + print("\n----- PREP: zero baseline (applied to subsequent phases) -----") + hand.start_tactile_stream(resultant=True, taxels=True, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_taxels) + pause("ensure NOTHING is touching any sensor") + offsets = hand.zero_tactile_sensors(num_samples=200) + max_baseline = 0.0 + for f in FINGERS: + if f in offsets and offsets[f]: + max_baseline = max(max_baseline, max(abs(t[2]) for t in offsets[f])) + print(f" Zero captured (max raw baseline |fz| was {max_baseline:.2f} N at hottest taxel)") + finally: + hand.stop_tactile_stream() + + +def phase_2_resultant_press(hand): + banner(2, "Auto-stream resultant + finger press") + hand.start_tactile_stream(resultant=True, taxels=False, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_forces) + time.sleep(0.5) + s0 = hand.get_tactile_stats() + time.sleep(5.0) + s1 = hand.get_tactile_stats() + rate = (s1.frames_ok - s0.frames_ok) / 5.0 + print(f" Frame rate: {rate:.0f} fps") + print(f" Stats: ok={s1.frames_ok} bad_lrc={s1.frames_bad_checksum} " + f"parse_err={s1.parse_errors} resyncs={s1.resyncs}") + + if rate < 50: + return False, f"frame rate {rate:.0f} fps < 50 (stream stalled?)" + if s1.frames_bad_checksum or s1.parse_errors or s1.resyncs: + return False, "non-zero error counters during idle stream" + + wiring = hand.config.finger_to_sensor_id + peaks_per_press = {} + warnings = [] + for f in FINGERS: + pause(f"press {f.upper()} (vary pressure to watch the live readout)") + all_peaks = live_press_resultant(hand, f, FINGERS, duration_s=1.5) + peaks_per_press[f] = all_peaks[f] + mismatch = detect_wiring_mismatch(f, all_peaks, wiring) + if mismatch: + print(mismatch) + warnings.append(f) + + weak = [f for f, p in peaks_per_press.items() if p < PRESS_THRESHOLD_N] + if warnings: + return False, f"wiring mismatch suspected on: {warnings} (see suggestions above)" + if weak: + return False, f"no/weak response on: {weak}" + return True, f"~{rate:.0f} fps clean, all fingers responded" + finally: + hand.stop_tactile_stream() + + +def phase_3_taxels_press(hand): + banner(3, "Auto-stream taxels + finger press") + hand.start_tactile_stream(resultant=False, taxels=True, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_taxels) + reading = hand.get_tactile_taxels() + for f in FINGERS: + if f not in reading: + return False, f"{f} missing from taxel frame" + n_expected = hand.get_tactile_configuration().num_taxels[f] + if len(reading[f]) != n_expected: + return False, (f"{f} taxel array length {len(reading[f])} " + f"!= reported {n_expected}") + if any(len(t) != 3 for t in reading[f]): + return False, f"{f} has malformed taxel vectors" + print(f" Taxel array shapes verified for all 5 fingers") + + wiring = hand.config.finger_to_sensor_id + peaks_per_press = {} + warnings = [] + for f in FINGERS: + pause(f"press {f.upper()} (move your finger around to light up different taxels)") + n = hand.get_tactile_configuration().num_taxels[f] + all_peaks = live_press_taxels(hand, f, FINGER_TO_ROLE[f], n, FINGERS, duration_s=2.5) + peaks_per_press[f] = all_peaks[f] + mismatch = detect_wiring_mismatch(f, all_peaks, wiring) + if mismatch: + print(mismatch) + warnings.append(f) + + weak = [f for f, p in peaks_per_press.items() if p < PRESS_THRESHOLD_N] + if warnings: + return False, f"wiring mismatch suspected on: {warnings} (see suggestions above)" + if weak: + return False, f"no/weak taxel response on: {weak}" + return True, "all fingers show per-taxel response" + finally: + hand.stop_tactile_stream() + + +def phase_4_combined(hand): + banner(4, "Combined mode (resultant + taxels)") + hand.start_tactile_stream(resultant=True, taxels=True, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_forces) + forces = hand.get_tactile_forces() + taxels = hand.get_tactile_taxels() + if forces is None or taxels is None: + return False, f"combined snapshot missing: forces={forces is not None} taxels={taxels is not None}" + + s0 = hand.get_tactile_stats() + time.sleep(3.0) + s1 = hand.get_tactile_stats() + rate = (s1.frames_ok - s0.frames_ok) / 3.0 + print(f" Frame rate (combined): {rate:.0f} fps") + if rate < 50: + return False, f"combined frame rate {rate:.0f} fps < 50 (stream stalled?)" + return True, f"both data types in single snapshot, ~{rate:.0f} fps" + finally: + hand.stop_tactile_stream() + + +def phase_5_zeroing(hand): + banner(5, "Zeroing") + pause("ensure NOTHING is touching any sensor") + hand.start_tactile_stream(resultant=True, taxels=True, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_taxels) + offsets = hand.zero_tactile_sensors(num_samples=200) + print(" Captured offsets per finger (avg |fz| per taxel):") + for f in FINGERS: + if f in offsets and offsets[f]: + fz_vals = [abs(t[2]) for t in offsets[f]] + avg = sum(fz_vals) / len(fz_vals) + print(f" {f:7s} avg={avg:.3f} N ({len(fz_vals)} taxels)") + + time.sleep(0.2) + forces = hand.get_tactile_forces() + max_resting = max(abs(forces[f][2]) for f in FINGERS) if forces else None + print(f" Max resting |fz| after zero: {max_resting:.3f} N") + if max_resting is None or max_resting > ZERO_TOLERANCE_N: + return False, f"resting fz not near zero (max={max_resting})" + + hand.clear_tactile_zero() + time.sleep(0.2) + forces = hand.get_tactile_forces() + if forces is None: + return False, "no frame after clear_tactile_zero" + + return True, f"zero captured, applied (max resting={max_resting:.2f}N), cleared" + finally: + hand.stop_tactile_stream() + + +def phase_6_lifecycle(hand): + banner(6, "Lifecycle: stop -> restart in new mode") + + print(" [1/4] Start resultant-only stream...") + hand.start_tactile_stream(resultant=True, taxels=False, min_sensors=1) + wait_for_frame(hand.get_tactile_forces) + print(" OK - forces frame received") + + print(" [2/4] Stop stream and verify cache cleared...") + hand.stop_tactile_stream() + if hand.get_tactile_forces() is not None: + return False, "stale forces after stop_tactile_stream" + print(" OK - cache cleared") + + print(" [3/4] Restart in taxels-only mode...") + hand.start_tactile_stream(resultant=False, taxels=True, min_sensors=1) + try: + wait_for_frame(hand.get_tactile_taxels) + if hand.get_tactile_taxels() is None: + return False, "no taxels after restart in taxels-only mode" + print(" OK - taxels frame received") + + print(" [4/4] Verify mode isolation (no resultant cache)...") + if hand.get_tactile_forces() is not None: + return False, "resultant cache populated in taxels-only mode" + print(" OK - resultant cache empty") + + return True, "stop -> restart in different mode works" + finally: + hand.stop_tactile_stream() + + +def print_summary(results): + print("\n===== SUMMARY =====") + for n in sorted(results): + ok, detail = results[n] + flag = "PASS" if ok else "FAIL" + print(f" Phase {n}: {flag} - {detail}") + if all(ok for ok, _ in results.values()): + print("\nAll phases passed.") + else: + print("\nOne or more phases failed.") + + +def main(): + parser = argparse.ArgumentParser(description=__doc__.split("\n")[0]) + parser.add_argument( + "config_path", + nargs="?", + default=None, + help="Path to the hand model directory or config.yaml", + ) + args = parser.parse_args() + + hand = OrcaHandTouch(config_path=args.config_path) + print(f"Sensor port: {hand.config.sensor_port}") + print(f"Baudrate: {hand.config.sensor_baudrate}") + print(f"Wiring: {hand.config.finger_to_sensor_id}") + + ok, msg = hand.connect_sensors_only() + print(msg) + if not ok: + return + + results = {} + phases_after_prep = [ + (2, phase_2_resultant_press), + (3, phase_3_taxels_press), + (4, phase_4_combined), + (5, phase_5_zeroing), + (6, phase_6_lifecycle), + ] + try: + try: + results[1] = phase_1_enumerate(hand) + except Exception as e: + results[1] = (False, f"raised {type(e).__name__}: {e}") + + if results[1][0]: + try: + prep_zero_baseline(hand) + except Exception as e: + print(f" WARNING: zero baseline prep failed ({type(e).__name__}: {e}); " + "subsequent phases will see raw readings") + + for n, fn in phases_after_prep: + try: + results[n] = fn(hand) + except Exception as e: + results[n] = (False, f"raised {type(e).__name__}: {e}") + finally: + hand.disconnect() + + print_summary(results) + + +if __name__ == "__main__": + main() diff --git a/tests/test_protocol.py b/tests/test_protocol.py new file mode 100644 index 00000000..5b0196a2 --- /dev/null +++ b/tests/test_protocol.py @@ -0,0 +1,521 @@ +"""Tests for the protocol codec layer (pure functions, no I/O).""" + +import struct + +import pytest + +from orca_core.hardware.sensing.protocol import ( + calculate_checksum, + validate_auto_frame_lrc, + read_response_body_size, + build_read_request, + build_write_request, + parse_read_response, + parse_write_response, + extract_write_response_data_length, + extract_auto_frame_eff_len, + unpack_auto_payload, + compute_resultant_payload_size, + compute_taxel_payload_size, + compute_combined_payload_size, + decode_resultant_auto, + decode_taxels_auto, + decode_combined_auto, + decode_resultant_register, + decode_connected_sensors, + decode_num_taxels, + decode_auto_data_type, + encode_auto_data_type, + encode_combined_auto_for_mock, + encode_resultant_auto_for_mock, +) +from orca_core.hardware.sensing.constants import ( + ADDR_NUM_TAXELS_LENGTH, + ADDR_NUM_TAXELS_START, + DEFAULT_TAXEL_COUNTS, + PROTOCOL_HEADER_REQUEST, + PROTOCOL_HEADER_RESPONSE, + PROTOCOL_HEADER_AUTO, + FUNC_CODE_READ, + FUNC_CODE_WRITE, + AUTO_DATA_RESULTANT, + AUTO_DATA_TAXELS, + BYTES_PER_RESULTANT, + BYTES_PER_TAXEL, + MAX_AUTO_FRAME_EFF_LEN, + SLOT_DISTAL_TAXEL_REGISTER_OFFSETS, +) + + +ID_TO_FINGER = {0: "thumb", 1: "index", 2: "middle", 3: "ring", 4: "pinky"} + + +def _build_read_response(data: bytes) -> bytes: + meta = bytes([0x00, FUNC_CODE_READ]) + (0x0010).to_bytes(2, "little") + len(data).to_bytes(2, "little") + frame_wo_lrc = PROTOCOL_HEADER_RESPONSE + meta + data + return frame_wo_lrc + bytes([calculate_checksum(frame_wo_lrc)]) + + +def _build_write_response(status: int) -> bytes: + meta = bytes([0x00, FUNC_CODE_WRITE]) + (0x0017).to_bytes(2, "little") + (1).to_bytes(2, "little") + frame_wo_lrc = PROTOCOL_HEADER_RESPONSE + meta + bytes([status]) + return frame_wo_lrc + bytes([calculate_checksum(frame_wo_lrc)]) + + +# --------------------------------------------------------------------------- +# Checksum +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("frame,expected", [ + (b"\x01\x02\x03", 0xFA), + (b"", 0), + (b"\x01", 0xFF), + (b"\xAA\x55\x00\x03\x10\x00\x04\x00", 0xEA), +]) +def test_calculate_checksum(frame, expected): + """Each case checks the concrete LRC value AND the LRC invariant + (frame + checksum sums to 0 mod 256).""" + checksum = calculate_checksum(frame) + assert checksum == expected + assert (sum(frame) + checksum) & 0xFF == 0 + + +@pytest.mark.parametrize("lrc,expected", [ + pytest.param(None, True, id="matching"), # None = compute the correct LRC + pytest.param(0xFF, False, id="bad"), +]) +def test_validate_auto_frame_lrc(lrc, expected): + meta = b"\x00" + (3).to_bytes(2, "little") + payload = b"\x00\x01\x02" + if lrc is None: + lrc = calculate_checksum(PROTOCOL_HEADER_AUTO + meta + payload) + assert validate_auto_frame_lrc(meta, payload, lrc) is expected + + +# --------------------------------------------------------------------------- +# Frame size helpers +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("count,expected", [ + (4, 11), # meta(6) + data(4) + LRC(1) + (1, 8), +]) +def test_read_response_body_size(count, expected): + assert read_response_body_size(count) == expected + + +# --------------------------------------------------------------------------- +# Frame builders +# --------------------------------------------------------------------------- + +def test_build_read_request(): + frame = build_read_request(address=0x0010, count=4) + assert frame[:2] == PROTOCOL_HEADER_REQUEST + assert frame[2] == 0x00 + assert frame[3] == FUNC_CODE_READ + assert int.from_bytes(frame[4:6], "little") == 0x0010 + assert int.from_bytes(frame[6:8], "little") == 4 + assert calculate_checksum(frame[:-1]) == frame[-1] + + +def test_build_write_request(): + frame = build_write_request(address=0x0017, data=b"\x01") + assert frame[:2] == PROTOCOL_HEADER_REQUEST + assert frame[2] == 0x00 + assert frame[3] == FUNC_CODE_WRITE + assert int.from_bytes(frame[4:6], "little") == 0x0017 + assert int.from_bytes(frame[6:8], "little") == 1 + assert frame[8] == 0x01 + assert calculate_checksum(frame[:-1]) == frame[-1] + + +@pytest.mark.parametrize("address,count,error_match", [ + (0x0010, 0, "count must be > 0"), + (0x0010, -1, "count must be > 0"), + (0x10000, 1, "address"), + (-1, 1, "address"), +]) +def test_build_read_request_invalid_inputs(address, count, error_match): + with pytest.raises(ValueError, match=error_match): + build_read_request(address=address, count=count) + + +@pytest.mark.parametrize("address,data,error_match", [ + (0x0017, b"", "data must not be empty"), + (0x10000, b"\x01", "address"), +]) +def test_build_write_request_invalid_inputs(address, data, error_match): + with pytest.raises(ValueError, match=error_match): + build_write_request(address=address, data=data) + + +# --------------------------------------------------------------------------- +# Response frame parsers +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("data", [ + b"\xAB\xCD\xEF\x01", + b"\x42", +]) +def test_parse_read_response_extracts_data(data): + frame = _build_read_response(data) + assert parse_read_response(frame) == data + + +def test_parse_read_response_bad_lrc_raises(): + frame = bytearray(_build_read_response(b"\x01\x02")) + frame[-1] ^= 0xFF + with pytest.raises(IOError, match="LRC mismatch"): + parse_read_response(bytes(frame)) + + +def test_parse_read_response_too_short_raises(): + with pytest.raises(IOError, match="too short"): + parse_read_response(b"\xAA\x55\x00") + + +def test_parse_read_response_wrong_func_code_raises(): + meta = bytes([0x00, FUNC_CODE_WRITE]) + (0x0010).to_bytes(2, "little") + (1).to_bytes(2, "little") + frame_wo_lrc = PROTOCOL_HEADER_RESPONSE + meta + b"\x00" + frame = frame_wo_lrc + bytes([calculate_checksum(frame_wo_lrc)]) + with pytest.raises(IOError, match="Expected read response"): + parse_read_response(frame) + + +def test_parse_read_response_wrong_header_raises(): + frame = bytearray(_build_read_response(b"\x01\x02")) + frame[0:2] = PROTOCOL_HEADER_AUTO + frame[-1] = calculate_checksum(bytes(frame[:-1])) + with pytest.raises(IOError, match="Expected response header"): + parse_read_response(bytes(frame)) + + +def test_parse_write_response_success(): + parse_write_response(_build_write_response(status=0x00)) + + +def test_parse_write_response_failure_status_raises(): + with pytest.raises(IOError, match="Write failed"): + parse_write_response(_build_write_response(status=0x01)) + + +def test_parse_write_response_bad_lrc_raises(): + frame = bytearray(_build_write_response(status=0x00)) + frame[-1] ^= 0xFF + with pytest.raises(IOError, match="LRC mismatch"): + parse_write_response(bytes(frame)) + + +def test_parse_write_response_too_short_raises(): + with pytest.raises(IOError, match="too short"): + parse_write_response(b"\xAA\x55\x00") + + +def test_parse_write_response_truncated_payload_raises(): + # Frame claims 10 payload bytes but only has 1 + meta = bytes([0x00, FUNC_CODE_WRITE]) + (0x0017).to_bytes(2, "little") + (10).to_bytes(2, "little") + frame_wo_lrc = PROTOCOL_HEADER_RESPONSE + meta + b"\x00" + frame = frame_wo_lrc + bytes([calculate_checksum(frame_wo_lrc)]) + with pytest.raises(IOError, match="truncated"): + parse_write_response(frame) + + +def test_parse_write_response_wrong_header_raises(): + frame = bytearray(_build_write_response(status=0x00)) + frame[0:2] = PROTOCOL_HEADER_AUTO + frame[-1] = calculate_checksum(bytes(frame[:-1])) + with pytest.raises(IOError, match="Expected response header"): + parse_write_response(bytes(frame)) + + +def test_extract_write_response_data_length_known_value(): + # meta: reserved(1) + func(1) + addr(2) + nbytes(2) + meta = bytes([0x00, FUNC_CODE_WRITE, 0x17, 0x00, 0x03, 0x00]) + assert extract_write_response_data_length(meta) == 3 + + +def test_extract_write_response_data_length_wrong_size_raises(): + with pytest.raises(ValueError, match="must be 6 bytes"): + extract_write_response_data_length(b"\x00\x00\x00\x00") + + +# --------------------------------------------------------------------------- +# Auto-stream frame parsers +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("value", [42, MAX_AUTO_FRAME_EFF_LEN]) +def test_extract_auto_frame_eff_len_valid(value): + # meta layout: reserved(1) + eff_len(2 LE) = 3 bytes + meta = b"\x00" + value.to_bytes(2, "little") + assert extract_auto_frame_eff_len(meta) == value + + +def test_extract_auto_frame_eff_len_exceeds_max_raises(): + meta = b"\x00" + (MAX_AUTO_FRAME_EFF_LEN + 1).to_bytes(2, "little") + with pytest.raises(ValueError, match="Invalid eff_len"): + extract_auto_frame_eff_len(meta) + + +@pytest.mark.parametrize("payload,expected_err,expected_data", [ + (b"\x00\x01\x02\x03", 0, b"\x01\x02\x03"), + (b"\x05\xAB", 5, b"\xAB"), + (b"\x01", 1, b""), +]) +def test_unpack_auto_payload(payload, expected_err, expected_data): + err, data = unpack_auto_payload(payload) + assert err == expected_err + assert data == expected_data + + +# --------------------------------------------------------------------------- +# Payload size computation +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("n", [0, 1, 3, 5]) +def test_compute_resultant_payload_size(n): + assert compute_resultant_payload_size(n) == n * BYTES_PER_RESULTANT + + +def test_compute_taxel_payload_size(): + active = ["thumb", "index"] + num_taxels = {"thumb": 51, "index": 87} + assert compute_taxel_payload_size(active, num_taxels) == (51 + 87) * BYTES_PER_TAXEL + + +def test_compute_taxel_payload_size_missing_finger_raises(): + with pytest.raises(KeyError): + compute_taxel_payload_size(["thumb"], {"index": 87}) + + +def test_compute_combined_payload_size(): + active = ["thumb", "index"] + num_taxels = {"thumb": 51, "index": 87} + expected = 2 * BYTES_PER_RESULTANT + (51 + 87) * BYTES_PER_TAXEL + assert compute_combined_payload_size(active, num_taxels) == expected + + +# --------------------------------------------------------------------------- +# Payload decoders — auto-stream +# +# Wire format: +# Resultant per sensor: 6 bytes — 3 axes (fx, fy, fz) × 2-byte slot. +# Only the low byte carries data (signed int8 for fx/fy, unsigned uint8 +# for fz). High byte is padding the firmware fills with sign-extension +# of the low byte cast to int8 — must be ignored regardless of value. +# Taxel per element: 3 bytes — int8 fx, int8 fy, uint8 fz +# Combined frames interleave: [resultant_sensor_i, taxels_sensor_i, ...] +# Newtons = LSB count × 0.1 +# --------------------------------------------------------------------------- + +def _resultant_slot(fx: int, fy: int, fz: int, hi: int = 0x00) -> bytes: + """Build a 6-byte resultant slot from raw axis bytes plus a high-byte filler. + + Uses two's-complement low-byte encoding for fx/fy. The same `hi` byte goes + into all three high-byte positions, which lets parametrize cases prove the + decoder ignores high-byte content. + """ + return bytes([fx & 0xFF, hi, fy & 0xFF, hi, fz & 0xFF, hi]) + + +@pytest.mark.parametrize("hi_byte", [0x00, 0xFF, 0xA5]) +@pytest.mark.parametrize( + "raw,expected", + [ + ([(100, -50, 200)], {"thumb": [10.0, -5.0, 20.0]}), + ([(0, 0, 110)], {"thumb": [0.0, 0.0, 11.0]}), # fz < 128 (unambiguous) + ([(0, 0, 200)], {"thumb": [0.0, 0.0, 20.0]}), # fz > 127 (firmware sign-extends) + ([(0, 0, 250)], {"thumb": [0.0, 0.0, 25.0]}), # fz near uint8 saturation + ([(0, 0, 255)], {"thumb": [0.0, 0.0, 25.5]}), # fz at uint8 saturation + ( + [(10, 20, 30), (-10, -20, 40)], + {"thumb": [1.0, 2.0, 3.0], "index": [-1.0, -2.0, 4.0]}, + ), + ], +) +def test_decode_resultant_auto(raw, expected, hi_byte): + data = b"".join(_resultant_slot(fx, fy, fz, hi=hi_byte) for (fx, fy, fz) in raw) + fingers = list(expected.keys()) + assert decode_resultant_auto(data, fingers) == expected + + +def test_decode_resultant_auto_wrong_size_raises(): + with pytest.raises(ValueError, match="size mismatch"): + decode_resultant_auto(b"\x00" * 5, ["thumb"]) + + +def test_decode_resultant_auto_error_includes_hex(): + with pytest.raises(ValueError, match="first bytes"): + decode_resultant_auto(b"\xDE\xAD", ["thumb"]) + + +def test_decode_taxels_auto_known_values(): + data = struct.pack("bbB", 10, -5, 20) + struct.pack("bbB", 0, 0, 50) + result = decode_taxels_auto(data, ["thumb"], {"thumb": 2}) + assert len(result["thumb"]) == 2 + assert result["thumb"][0] == [1.0, -0.5, 2.0] + assert result["thumb"][1] == [0.0, 0.0, 5.0] + + +def test_decode_taxels_auto_wrong_size_raises(): + with pytest.raises(ValueError, match="size mismatch"): + decode_taxels_auto(b"\x00" * 5, ["thumb"], {"thumb": 2}) + + +def test_decode_combined_auto_interleaved(): + data = ( + _resultant_slot(100, 0, 50) + + struct.pack("bbB", 10, 0, 5) + + _resultant_slot(0, -100, 200, hi=0xFF) # index fz > 127, firmware sign-extends + + struct.pack("bbB", 0, -10, 20) + ) + forces, taxels = decode_combined_auto( + data, ["thumb", "index"], {"thumb": 1, "index": 1}, + ) + assert forces["thumb"] == [10.0, 0.0, 5.0] + assert forces["index"] == [0.0, -10.0, 20.0] + assert taxels["thumb"][0] == [1.0, 0.0, 0.5] + assert taxels["index"][0] == [0.0, -1.0, 2.0] + + +@pytest.mark.parametrize("forces", [ + {"thumb": [0.0, 0.0, 0.0]}, + {"thumb": [10.0, -5.0, 20.0]}, + {"thumb": [-12.8, 12.7, 25.5]}, # all axes at signed-byte boundary + {"thumb": [0.0, 0.0, 12.8]}, # fz where firmware high byte flips + {"thumb": [0.0, 0.0, 25.5]}, # fz at uint8 saturation + {"thumb": [4.2, -3.0, 20.0], "index": [-1.0, 0.5, 0.1]}, +]) +def test_resultant_encode_decode_roundtrip(forces): + fingers = list(forces.keys()) + wire = encode_resultant_auto_for_mock(forces, fingers) + assert decode_resultant_auto(wire, fingers) == forces + + +def test_combined_encode_decode_roundtrip(): + forces = {"thumb": [10.0, -5.0, 20.0], "index": [-1.0, 0.5, 25.5]} + taxels = {"thumb": [[1.0, -0.5, 2.0]], "index": [[0.0, 0.0, 0.5]]} + fingers = ["thumb", "index"] + wire = encode_combined_auto_for_mock(forces, taxels, fingers) + decoded_forces, decoded_taxels = decode_combined_auto( + wire, fingers, {"thumb": 1, "index": 1}, + ) + assert decoded_forces == forces + assert decoded_taxels == taxels + + +# --------------------------------------------------------------------------- +# Payload decoders — register block +# --------------------------------------------------------------------------- + +def test_decode_resultant_register_known_values(): + # Module index for thumb (slot 0): 0*4+2 = 2, byte offset = 12 + data = bytearray(168) + data[12:18] = _resultant_slot(100, -50, 200, hi=0xFF) # fz > 127 + result = decode_resultant_register(bytes(data), ["thumb"], {"thumb": 2}) + assert result["thumb"] == [10.0, -5.0, 20.0] + + +def test_decode_resultant_register_too_short_raises(): + with pytest.raises(ValueError, match="too short"): + decode_resultant_register(b"\x00" * 10, ["thumb"], {"thumb": 2}) + + +# --------------------------------------------------------------------------- +# Register decoders — connected sensors +# --------------------------------------------------------------------------- + +# Slot i bit mask: slot 0 → byte 0 bit 2, slot 1 → byte 0 bit 6, +# slot 2 → byte 1 bit 2, slot 3 → byte 1 bit 6, slot 4 → byte 2 bit 2. +@pytest.mark.parametrize("data,expected", [ + pytest.param( + bytes([0x44, 0x44, 0x04, 0x00]), + {"thumb": True, "index": True, "middle": True, "ring": True, "pinky": True}, + id="all_connected", + ), + pytest.param( + bytes([0x00, 0x00, 0x00, 0x00]), + {"thumb": False, "index": False, "middle": False, "ring": False, "pinky": False}, + id="none_connected", + ), + pytest.param( + bytes([0x04, 0x00, 0x04, 0x00]), + {"thumb": True, "index": False, "middle": False, "ring": False, "pinky": True}, + id="partial", + ), +]) +def test_decode_connected_sensors(data, expected): + assert decode_connected_sensors(data, ID_TO_FINGER) == expected + + +def test_decode_connected_sensors_too_short_raises(): + with pytest.raises(ValueError, match="Expected 4 bytes"): + decode_connected_sensors(b"\x00\x00", ID_TO_FINGER) + + +def test_decode_connected_sensors_wrong_mapping_size_raises(): + with pytest.raises(ValueError, match="must have 5 entries"): + decode_connected_sensors(bytes([0x00, 0x00, 0x00, 0x00]), {0: "thumb", 1: "index"}) + + +# --------------------------------------------------------------------------- +# Register decoders — num taxels +# --------------------------------------------------------------------------- + +def test_decode_num_taxels_known_values(): + """Pack DEFAULT_TAXEL_COUNTS at the configured slot register offsets, decode, + assert round-trip. Both the byte offsets and the values are derived from + constants the decoder reads, so the test follows the source of truth.""" + data = bytearray(ADDR_NUM_TAXELS_LENGTH) + for slot_id, addr in enumerate(SLOT_DISTAL_TAXEL_REGISTER_OFFSETS): + finger = ID_TO_FINGER[slot_id] + byte_offset = addr - ADDR_NUM_TAXELS_START + struct.pack_into(" TactileSensorConfiguration: + if taxel_counts is None: + taxel_counts = DEFAULT_TAXEL_COUNTS + if finger_to_sensor_id is None: + finger_to_sensor_id = {"thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4} + + connected = {f: (f in connected_fingers) for f in ALL_FINGERS} + num_taxels = {f: taxel_counts.get(f, 0) for f in connected_fingers} + module_indices = {f: compute_distal_module_index(finger_to_sensor_id[f]) for f in connected_fingers} + + return TactileSensorConfiguration( + connected=connected, + num_taxels=num_taxels, + module_indices=module_indices, + finger_to_sensor_id=finger_to_sensor_id, + ) + + +# --------------------------------------------------------------------------- +# Resultant forces — round-trip per finger +# --------------------------------------------------------------------------- + +FORCE_VECTORS = { + "thumb": [1.5, -2.0, 3.0], + "index": [0.1, 0.2, 0.3], + "middle": [-1.0, 0.5, 10.0], + "ring": [4.0, -4.0, 4.0], + "pinky": [0.0, 0.0, 0.5], +} + + +@pytest.mark.parametrize("finger", ALL_FINGERS) +def test_resultant_round_trip_per_finger(mock, finger): + mock.set_mock_forces(FORCE_VECTORS) + mock.start_auto_stream(resultant=True, taxels=False) + mock.wait_for_first_frame() + result, ts = mock.get_auto_latest() + mock.stop_auto_stream() + + assert ts is not None + assert result[finger] == FORCE_VECTORS[finger] + + +@pytest.mark.parametrize( + "subset", + [ + ["thumb"], + ["thumb", "pinky"], + ["index", "middle", "ring"], + ALL_FINGERS, + ], +) +def test_only_connected_sensors_returned(mock_factory, subset): + mock = mock_factory(subset) + mock.start_auto_stream(resultant=True, taxels=False) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest() + mock.stop_auto_stream() + + assert set(result.keys()) == set(subset) + + +# --------------------------------------------------------------------------- +# Combined mode +# --------------------------------------------------------------------------- + +def test_combined_mode_returns_both_streams(mock): + mock.set_mock_forces({f: [1.0, 0.0, 0.5] for f in ALL_FINGERS}) + mock.start_auto_stream(resultant=True, taxels=True) + mock.wait_for_first_frame() + forces, taxels, ts = mock.get_auto_latest_all() + mock.stop_auto_stream() + + assert ts is not None + assert set(forces.keys()) == set(ALL_FINGERS) + assert set(taxels.keys()) == set(ALL_FINGERS) + for finger in ALL_FINGERS: + assert forces[finger] == [1.0, 0.0, 0.5] + assert len(taxels[finger]) == DEFAULT_TAXEL_COUNTS[finger] + + +# --------------------------------------------------------------------------- +# Provider injection +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize( + "kind", + ["resultant", "taxel"], +) +def test_custom_provider_is_used(kind): + if kind == "resultant": + marker = [4.2, -3.0, 20.0] + mock = MockTactileClient( + connected_sensors=["thumb"], + resultant_provider=lambda: {"thumb": marker}, + ) + mock.connect() + mock.start_auto_stream(resultant=True, taxels=False) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest() + mock.stop_auto_stream() + mock.disconnect() + assert result["thumb"] == marker + else: + marker = [[9.9, -8.8, 7.7]] + mock = MockTactileClient( + connected_sensors=["thumb"], + taxel_counts={"thumb": 1}, + taxel_provider=lambda: {"thumb": marker}, + ) + mock.connect() + mock.start_auto_stream(resultant=False, taxels=True) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest_taxels() + mock.stop_auto_stream() + mock.disconnect() + assert result["thumb"] == marker + + +# --------------------------------------------------------------------------- +# Offset logic +# --------------------------------------------------------------------------- + +@pytest.mark.parametrize("finger", ALL_FINGERS) +def test_resultant_offsets_applied(mock_factory, finger): + mock = mock_factory([finger]) + mock.set_mock_forces({finger: [5.0, 3.0, 10.0]}) + mock.set_taxel_offsets({finger: [[1.0, 0.5, 2.0]]}) + + result = mock.read_resultant_force() + assert result[finger] == [4.0, 2.5, 8.0] + + +@pytest.mark.parametrize("finger", ALL_FINGERS) +def test_fz_clamped_to_zero(mock_factory, finger): + mock = mock_factory([finger]) + mock.set_mock_forces({finger: [0.0, 0.0, 1.0]}) + mock.set_taxel_offsets({finger: [[0.0, 0.0, 5.0]]}) + + result = mock.read_resultant_force() + assert result[finger][2] == 0.0 + + +def test_clear_offsets(mock_factory): + mock = mock_factory(["thumb"]) + mock.set_mock_forces({"thumb": [5.0, 3.0, 10.0]}) + mock.set_taxel_offsets({"thumb": [[1.0, 0.5, 2.0]]}) + mock.clear_taxel_offsets() + + result = mock.read_resultant_force() + assert result["thumb"] == [5.0, 3.0, 10.0] + + +@pytest.mark.parametrize("finger", ALL_FINGERS) +def test_stream_offsets_applied(mock_factory, finger): + mock = mock_factory([finger]) + mock.set_mock_forces({finger: [5.0, 3.0, 10.0]}) + mock.set_taxel_offsets({finger: [[1.0, 0.5, 2.0]]}) + mock.start_auto_stream(resultant=True, taxels=False) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest() + mock.stop_auto_stream() + + assert result[finger] == [4.0, 2.5, 8.0] + + +def test_taxel_offsets_applied_in_stream(mock_factory): + taxels = [[2.0, 1.0, 5.0], [3.0, 2.0, 8.0]] + offsets = [[0.5, 0.5, 1.0], [1.0, 1.0, 2.0]] + mock = mock_factory( + ["thumb"], + taxel_counts={"thumb": 2}, + taxel_provider=lambda: {"thumb": [list(t) for t in taxels]}, + ) + mock.set_taxel_offsets({"thumb": offsets}) + mock.start_auto_stream(resultant=False, taxels=True) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest_taxels() + mock.stop_auto_stream() + + assert result["thumb"] == [[1.5, 0.5, 4.0], [2.0, 1.0, 6.0]] + + +# --------------------------------------------------------------------------- +# Error paths +# --------------------------------------------------------------------------- + +def test_read_before_connect_raises(): + mock = MockTactileClient(connected_sensors=ALL_FINGERS) + with pytest.raises(OSError, match="connect"): + mock.read_resultant_force() + + +def test_get_auto_latest_before_stream_returns_none(mock): + result, ts = mock.get_auto_latest() + assert result is None + assert ts is None + + +# --------------------------------------------------------------------------- +# TactileSensorConfiguration ordering +# --------------------------------------------------------------------------- + +def test_slot_order_default(): + config = _make_config(ALL_FINGERS) + assert config.active_sensors == ["thumb", "index", "middle", "ring", "pinky"] + + +def test_slot_order_custom_mapping(): + custom_map = {"thumb": 1, "index": 3, "middle": 0, "ring": 2, "pinky": 4} + config = _make_config(ALL_FINGERS, finger_to_sensor_id=custom_map) + assert config.active_sensors == ["middle", "thumb", "ring", "index", "pinky"] + + +def test_slot_order_subset_preserves_order(): + config = _make_config(["pinky", "thumb"]) + assert config.active_sensors == ["thumb", "pinky"]