From 639823898e3dfd8e35a2d6db9790a14318d6a8b6 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Fri, 27 Mar 2026 19:16:29 +0100 Subject: [PATCH 01/20] Add tactile sensing integration with OrcaHandTouch subclass --- orca_core/__init__.py | 4 + orca_core/hand_config.py | 62 + .../hardware/sensing/mock_sensor_client.py | 588 ++++++ .../sensing/models/sensor_models.yaml | 15 + .../models/touch-sensor-finger/config.yaml | 265 +++ .../models/touch-sensor-pinky/config.yaml | 157 ++ .../models/touch-sensor-thumb/config.yaml | 157 ++ orca_core/hardware/sensing/sensor_client.py | 1636 +++++++++++++++++ .../hardware/sensing/taxel_coordinates.py | 85 + orca_core/hardware_hand.py | 91 +- .../models/v2/orcahand-touch/config.yaml | 211 +++ pyproject.toml | 7 + scripts/tactile_sensing_ui/README.md | 26 + scripts/tactile_sensing_ui/static/script.js | 879 +++++++++ scripts/tactile_sensing_ui/static/style.css | 628 +++++++ scripts/tactile_sensing_ui/tactile_ui.py | 341 ++++ .../tactile_sensing_ui/templates/index.html | 230 +++ tests/test_tactile_sensor.py | 381 ++++ 18 files changed, 5762 insertions(+), 1 deletion(-) create mode 100644 orca_core/hardware/sensing/mock_sensor_client.py create mode 100644 orca_core/hardware/sensing/models/sensor_models.yaml create mode 100644 orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml create mode 100644 orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml create mode 100644 orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml create mode 100644 orca_core/hardware/sensing/sensor_client.py create mode 100644 orca_core/hardware/sensing/taxel_coordinates.py create mode 100644 orca_core/models/v2/orcahand-touch/config.yaml create mode 100644 scripts/tactile_sensing_ui/README.md create mode 100644 scripts/tactile_sensing_ui/static/script.js create mode 100644 scripts/tactile_sensing_ui/static/style.css create mode 100755 scripts/tactile_sensing_ui/tactile_ui.py create mode 100644 scripts/tactile_sensing_ui/templates/index.html create mode 100644 tests/test_tactile_sensor.py 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/hand_config.py b/orca_core/hand_config.py index ab0ac2ca..147a0615 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 @@ -322,4 +323,65 @@ def __post_init__(self) -> None: self.validate_config() +FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] +VALID_SENSOR_IDS = set(range(5)) + + +@dataclass(frozen=True) +class OrcaHandTouchConfig(OrcaHandConfig): + """ORCA hand configuration with tactile sensor support.""" + + sensor_port: str = "/dev/ttyACM0" + sensor_baudrate: int = 921600 + finger_to_sensor_id: Dict[str, int] = field( + default_factory=lambda: { + "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4, + } + ) + + @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/sensing/mock_sensor_client.py b/orca_core/hardware/sensing/mock_sensor_client.py new file mode 100644 index 00000000..281d8925 --- /dev/null +++ b/orca_core/hardware/sensing/mock_sensor_client.py @@ -0,0 +1,588 @@ +# ============================================================================== +# 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 dataclasses import dataclass, field +from typing import Optional +import threading +import time +import random +import logging + +logger = logging.getLogger(__name__) + +FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] + +# Default taxel counts loaded from sensor model config +def _get_default_taxel_counts() -> dict[str, int]: + try: + from orca_core.hardware.sensing.taxel_coordinates import get_taxel_counts + return get_taxel_counts() + except Exception: + return {"thumb": 51, "index": 87, "middle": 87, "ring": 87, "pinky": 51} + +DEFAULT_TAXEL_COUNTS = _get_default_taxel_counts() + + +class NoSensorsAvailableError(Exception): + """Raised when no sensors are available for communication.""" + pass + + +@dataclass +class AutoStreamStats: + frames_ok: int = 0 + frames_bad_lrc: int = 0 + resyncs: int = 0 + last_error_code: int = 0 + parse_ok: int = 0 + parse_errors: int = 0 + last_eff_len: int = 0 + last_payload_len: int = 0 + consecutive_errors: int = 0 + reconfiguration_count: int = 0 + + +@dataclass +class SensorConfiguration: + """Snapshot of connected sensors and their properties.""" + connected: dict[str, bool] = field(default_factory=dict) + num_taxels: dict[str, int] = field(default_factory=dict) + module_indices: dict[str, int] = field(default_factory=dict) + expected_payload_size_resultant: int = 0 + expected_payload_size_taxels: int = 0 + expected_payload_size_combined: int = 0 + timestamp: float = 0.0 + finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: { + "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 + }) + + @property + def active_sensors(self) -> list[str]: + """List of currently connected sensors sorted by hardware slot order.""" + 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: + """Number of currently connected sensors.""" + 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 MockSensorClient: + """Mock client for simulating tactile sensor communication in tests. + + This class provides the same interface as SensorClient but returns + simulated data instead of communicating with real hardware. Useful for: + - Automated testing without hardware + - Development and debugging + - CI/CD pipelines + + The simulated data can be controlled via: + - set_mock_forces(): Set specific force values to return + - set_mock_taxels(): Set specific taxel values to return + - set_connected_sensors(): Configure which sensors appear connected + - set_noise_level(): Add random noise to simulated data + """ + + def __init__(self, + port: str = '/dev/ttyUSB0', + baudrate: int = 921600, + connected_sensors: Optional[list[str]] = None, + finger_to_sensor_id: Optional[dict[str, int]] = None): + """Initialize mock sensor client. + + Args: + port: Serial port (ignored, for API compatibility) + baudrate: Baudrate (ignored, for API compatibility) + connected_sensors: List of sensor names to simulate as connected. + Defaults to ["thumb", "index", "middle"] + finger_to_sensor_id: Finger-to-sensor-id mapping (ignored, for API compatibility) + """ + self.port = port + self.baudrate = baudrate + self._connected = False + + # Configure which sensors appear connected + if connected_sensors is None: + connected_sensors = ["thumb", "index", "middle"] + self._simulated_connected = {f: f in connected_sensors for f in FINGER_NAMES} + self._simulated_taxel_counts = { + f: DEFAULT_TAXEL_COUNTS[f] if self._simulated_connected[f] else 0 + for f in FINGER_NAMES + } + + # Mock data storage + self._mock_forces: dict[str, list[float]] = {} + self._mock_taxels: dict[str, list[list[float]]] = {} + self._noise_level = 0.0 + self._hardware_version = "MOCK_V1.0.0" + + # Auto-stream state + self._sensor_config: Optional[SensorConfiguration] = None + self._auto_thread: Optional[threading.Thread] = 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._auto_rate_hz = 100 # Simulated update rate + + # Initialize default mock data + self._initialize_mock_data() + + def _initialize_mock_data(self): + """Initialize default mock force and taxel data.""" + for finger in FINGER_NAMES: + if self._simulated_connected[finger]: + self._mock_forces[finger] = [0.0, 0.0, 0.0] + taxel_count = self._simulated_taxel_counts[finger] + self._mock_taxels[finger] = [[0.0, 0.0, 0.0] for _ in range(taxel_count)] + + # ========================================================================= + # Mock Control Methods (for test setup) + # ========================================================================= + + def set_connected_sensors(self, sensors: list[str]): + """Configure which sensors appear as connected. + + Args: + sensors: List of finger names to simulate as connected + """ + self._simulated_connected = {f: f in sensors for f in FINGER_NAMES} + self._simulated_taxel_counts = { + f: DEFAULT_TAXEL_COUNTS[f] if self._simulated_connected[f] else 0 + for f in FINGER_NAMES + } + self._initialize_mock_data() + + # Update configuration if already connected + if self._connected: + self._sensor_config = self._get_configuration() + + def set_mock_forces(self, forces: dict[str, list[float]]): + """Set the force values to return for each sensor. + + Args: + forces: Dict mapping finger names to [fx, fy, fz] values + """ + for finger, force in forces.items(): + if finger in FINGER_NAMES and len(force) == 3: + self._mock_forces[finger] = list(force) + + def set_mock_taxels(self, taxels: dict[str, list[list[float]]]): + """Set the taxel values to return for each sensor. + + Args: + taxels: Dict mapping finger names to list of [fx, fy, fz] per taxel + """ + for finger, data in taxels.items(): + if finger in FINGER_NAMES: + self._mock_taxels[finger] = [list(t) for t in data] + + def set_noise_level(self, level: float): + """Set random noise level to add to returned data. + + Args: + level: Standard deviation of Gaussian noise to add (in Newtons) + """ + self._noise_level = level + + def set_hardware_version(self, version: str): + """Set the hardware version string to return. + + Args: + version: Version string to return from read_hardware_version() + """ + self._hardware_version = version + + def set_auto_rate(self, rate_hz: float): + """Set the simulated auto-stream update rate. + + Args: + rate_hz: Updates per second for auto-stream simulation + """ + self._auto_rate_hz = rate_hz + + # ========================================================================= + # Connection Methods + # ========================================================================= + + @property + def is_connected(self) -> bool: + """Check if client is connected.""" + return self._connected + + def connect(self): + """Simulate connecting to sensor device.""" + if self.is_connected: + return + + self._connected = True + logger.info(f"[MOCK] Connected to sensor at {self.port}") + + # Build initial configuration + self._sensor_config = self._get_configuration() + logger.info(f"[MOCK] Initial configuration: {self._sensor_config}") + + def disconnect(self): + """Simulate disconnecting from sensor device.""" + if not self.is_connected: + return + + self.stop_auto_stream() + self._connected = False + logger.info("[MOCK] Disconnected from sensor") + + # ========================================================================= + # Sensor Information Methods + # ========================================================================= + + def read_hardware_version(self) -> str: + """Return mock hardware version.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + return self._hardware_version + + def read_connected_sensors(self) -> dict[str, bool]: + """Return simulated connected sensor status.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + return dict(self._simulated_connected) + + def read_num_taxels(self) -> dict[str, int]: + """Return simulated taxel counts.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + return dict(self._simulated_taxel_counts) + + def read_auto_data_type(self) -> dict: + """Return simulated auto data type configuration.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + val = (0x01 if self._auto_mode_resultant else 0) | (0x02 if self._auto_mode_taxels else 0) + return { + "raw": f"{val:08b}", + "resulting_force": self._auto_mode_resultant, + "individual_taxels_force": self._auto_mode_taxels, + } + + def get_sensor_configuration(self) -> Optional[SensorConfiguration]: + """Get the current sensor configuration snapshot.""" + return self._sensor_config + + def _get_configuration(self) -> SensorConfiguration: + """Build configuration from current simulated state.""" + connected = dict(self._simulated_connected) + num_taxels = dict(self._simulated_taxel_counts) + + module_indices = {} + for i, finger in enumerate(FINGER_NAMES): + if connected.get(finger, False): + module_indices[finger] = i * 4 + 2 + + num_active = sum(1 for c in connected.values() if c) + expected_resultant = num_active * 6 + expected_taxels = sum( + num_taxels.get(finger, 0) * 3 + for finger, is_connected in connected.items() + if is_connected + ) + expected_combined = expected_resultant + expected_taxels + + return SensorConfiguration( + connected=connected, + num_taxels=num_taxels, + module_indices=module_indices, + expected_payload_size_resultant=expected_resultant, + expected_payload_size_taxels=expected_taxels, + expected_payload_size_combined=expected_combined, + timestamp=time.time() + ) + + # ========================================================================= + # Force Reading Methods + # ========================================================================= + + def _add_noise(self, value: float) -> float: + """Add Gaussian noise to a value.""" + if self._noise_level > 0: + return value + random.gauss(0, self._noise_level) + return value + + def _get_mock_forces(self) -> dict[str, list[float]]: + """Get mock forces with optional noise.""" + result = {} + for finger in FINGER_NAMES: + if self._simulated_connected.get(finger, False): + base = self._mock_forces.get(finger, [0.0, 0.0, 0.0]) + result[finger] = [ + round(self._add_noise(base[0]), 1), + round(self._add_noise(base[1]), 1), + round(self._add_noise(base[2]), 1), + ] + return result + + def _get_mock_taxels(self) -> dict[str, list[list[float]]]: + """Get mock taxels with optional noise.""" + result = {} + for finger in FINGER_NAMES: + if self._simulated_connected.get(finger, False): + base_taxels = self._mock_taxels.get(finger, []) + result[finger] = [ + [ + round(self._add_noise(t[0]), 2), + round(self._add_noise(t[1]), 2), + round(self._add_noise(t[2]), 2), + ] + for t in base_taxels + ] + return result + + def read_resulting_force(self) -> dict[str, list[float]]: + """Return simulated resultant forces.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + return self._get_mock_forces() + + # ========================================================================= + # Auto-Stream Control Methods + # ========================================================================= + + def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: + """Configure data types for auto stream.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + self._auto_mode_resultant = resultant + self._auto_mode_taxels = taxels + + def enable_auto_data_transmission(self) -> None: + """Enable auto data transmission (no-op in mock).""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + def disable_auto_data_transmission(self) -> None: + """Disable auto data transmission (no-op in mock).""" + if not self.is_connected: + raise OSError("Must call connect() first.") + + def reboot(self) -> None: + """Simulate sensor reboot.""" + if not self.is_connected: + raise OSError("Must call connect() first.") + logger.info("[MOCK] Sensor reboot simulated") + + # ========================================================================= + # Auto-Stream Methods + # ========================================================================= + + def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): + """Background thread that simulates auto-stream data generation.""" + interval = 1.0 / self._auto_rate_hz + + while self._auto_running.is_set(): + try: + parsed_resultant = None + parsed_taxels = None + + if parse_resultant: + parsed_resultant = self._get_mock_forces() + if parse_taxels: + parsed_taxels = self._get_mock_taxels() + + with self._auto_lock: + if parse_resultant: + self._auto_latest = parsed_resultant + if parse_taxels: + self._auto_latest_taxels = parsed_taxels + self._auto_latest_ts = time.time() + self._auto_stats.frames_ok += 1 + self._auto_stats.parse_ok += 1 + + time.sleep(interval) + + except Exception as e: + logger.error(f"[MOCK] Error in auto reader: {e}") + with self._auto_lock: + self._auto_stats.parse_errors += 1 + time.sleep(interval) + + logger.info("[MOCK] Auto reader loop exited") + + def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1): + """Start simulated auto-stream mode. + + Args: + resultant: Include resultant force data + taxels: Include taxel data + min_sensors: Minimum sensors required + + Raises: + OSError: If not connected + NoSensorsAvailableError: If fewer than min_sensors available + ValueError: If neither resultant nor taxels enabled + """ + 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 + + self._sensor_config = self._get_configuration() + + if self._sensor_config.num_active_sensors < min_sensors: + raise NoSensorsAvailableError( + f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " + f"need at least {min_sensors}" + ) + + mode_str = [] + if resultant: + mode_str.append("resultant") + if taxels: + mode_str.append("taxels") + logger.info( + f"[MOCK] Starting auto-stream with {self._sensor_config}, " + f"mode={'+'.join(mode_str)}" + ) + + 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 simulated auto-stream mode.""" + self._auto_running.clear() + + if self._auto_thread is not None: + self._auto_thread.join(timeout=1.0) + self._auto_thread = None + + with self._auto_lock: + self._auto_latest = None + self._auto_latest_taxels = None + self._auto_latest_ts = None + + def get_auto_latest(self): + """Get latest simulated resultant force data.""" + with self._auto_lock: + return self._auto_latest, self._auto_latest_ts + + def get_auto_latest_taxels(self): + """Get latest simulated taxel data.""" + with self._auto_lock: + return self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_latest_all(self): + """Get all latest simulated data.""" + with self._auto_lock: + return self._auto_latest, self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_stats(self): + """Get auto-stream statistics.""" + with self._auto_lock: + return self._auto_stats + + # ========================================================================= + # Context Manager Support + # ========================================================================= + + def __enter__(self): + """Enable use as context manager.""" + if not self.is_connected: + self.connect() + return self + + def __exit__(self, *args): + """Enable use as context manager.""" + self.disconnect() + + def __del__(self): + """Cleanup on destruction.""" + try: + self.disconnect() + except Exception: + pass + + +if __name__ == "__main__": + # Simple test of mock client + logging.basicConfig(level=logging.INFO) + + print("=== Mock Sensor Client Test ===\n") + + with MockSensorClient(connected_sensors=["thumb", "index", "middle"]) as client: + print(f"Hardware version: {client.read_hardware_version()}") + print(f"Connected sensors: {client.read_connected_sensors()}") + print(f"Taxel counts: {client.read_num_taxels()}") + print(f"Configuration: {client.get_sensor_configuration()}") + + # Test request-response mode + print("\n--- Request-Response Mode ---") + forces = client.read_resulting_force() + print(f"Forces: {forces}") + + # Set specific mock values + client.set_mock_forces({ + "thumb": [1.0, 0.5, 2.0], + "index": [0.0, 0.0, 1.5], + "middle": [-0.5, 0.2, 0.8], + }) + forces = client.read_resulting_force() + print(f"Forces (with mock values): {forces}") + + # Test with noise + client.set_noise_level(0.1) + forces = client.read_resulting_force() + print(f"Forces (with noise): {forces}") + client.set_noise_level(0.0) + + # Test auto-stream mode + print("\n--- Auto-Stream Mode (resultant only) ---") + client.start_auto_stream(resultant=True, taxels=False) + time.sleep(0.1) + for _ in range(5): + forces, ts = client.get_auto_latest() + if forces: + print(f"[{ts:.3f}] {forces}") + time.sleep(0.05) + client.stop_auto_stream() + + # Test combined mode + print("\n--- Auto-Stream Mode (combined) ---") + client.start_auto_stream(resultant=True, taxels=True) + time.sleep(0.1) + forces, taxels, ts = client.get_auto_latest_all() + if forces: + print(f"Forces: {forces}") + print(f"Taxel counts: {({f: len(t) for f, t in taxels.items()}) if taxels else 'None'}") + client.stop_auto_stream() + + # Show stats + stats = client.get_auto_stats() + print(f"\nStats: frames_ok={stats.frames_ok}, parse_ok={stats.parse_ok}") + + print("\n=== Test Complete ===") diff --git a/orca_core/hardware/sensing/models/sensor_models.yaml b/orca_core/hardware/sensing/models/sensor_models.yaml new file mode 100644 index 00000000..0fb3a634 --- /dev/null +++ b/orca_core/hardware/sensing/models/sensor_models.yaml @@ -0,0 +1,15 @@ +# Finger-to-sensor-model mapping +# Change these to switch which sensor model is used on each finger. +# Model names must match a subdirectory in this folder. +# +# Available models: +# touch-sensor-thumb - ORCA Fingertip Thumb (51 taxels) +# touch-sensor-finger - ORCA Fingertip Finger (87 taxels) +# touch-sensor-pinky - ORCA Fingertip Pinky (51 taxels) + +finger_models: + thumb: touch-sensor-thumb + index: touch-sensor-finger + middle: touch-sensor-finger + ring: touch-sensor-finger + pinky: touch-sensor-pinky diff --git a/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml new file mode 100644 index 00000000..c15a42cb --- /dev/null +++ b/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml @@ -0,0 +1,265 @@ +name: touch-sensor-finger +description: ORCA Fingertip - Finger +num_taxels: 87 +coordinates: +- x: 4.03569563 + y: 28.08244307 + z: 3.72577291 +- x: 6.62767274 + y: 24.49442143 + z: 2.9712037 +- x: 6.53635781 + y: 24.33135486 + z: 5.83911297 +- x: 4.09826233 + y: 27.00584015 + z: 7.05315498 +- x: 5.72524942 + y: 23.13913822 + z: 8.34520194 +- x: 1.215e-05 + y: 23.87765425 + z: 10.75159362 +- x: 3.13511549 + y: 23.80404808 + z: 10.10288971 +- x: -0.00135166 + y: 28.02324631 + z: 7.86111074 +- x: -0.00041269 + y: 29.21465893 + z: 3.96386436 +- x: 7.38872988 + y: -1.83843131 + z: 9.31075593 +- x: 4.25469072 + y: -1.83842924 + z: 12.2044915 +- x: 0.00139008 + y: -1.8375095 + z: 12.76352439 +- x: 0.00118076 + y: 1.36767275 + z: 11.08371687 +- x: 0.0010128 + y: 4.17300381 + z: 9.76182211 +- x: 0.00088397 + y: 7.66127894 + z: 8.9032549 +- x: 0.0007587 + y: 10.66833351 + z: 9.60998857 +- x: 0.00054434 + y: 14.13917019 + z: 10.6290711 +- x: 0.00037025 + y: 17.19870209 + z: 11.12688274 +- x: 0.00017672 + y: 20.81310695 + z: 11.20073003 +- x: 6.37805738 + y: 20.12836446 + z: 8.46599775 +- x: 6.19975695 + y: 16.80815911 + z: 8.75863336 +- x: 6.09392303 + y: 13.78451673 + z: 8.28088525 +- x: 5.81396004 + y: 10.82948586 + z: 7.59170343 +- x: 5.67589675 + y: 7.37239728 + z: 7.04781175 +- x: 5.6229397 + y: 4.30120402 + z: 7.98710121 +- x: 6.51658251 + y: 1.00320616 + z: 8.81523868 +- x: 2.90784487 + y: 10.8145338 + z: 9.36445453 +- x: 3.1353787 + y: 4.13280498 + z: 9.48359329 +- x: 3.59420968 + y: 1.33607952 + z: 10.72327448 +- x: 2.85383295 + y: 7.20978104 + z: 8.63989556 +- x: 3.69669249 + y: 17.22514737 + z: 10.57684617 +- x: 3.05173451 + y: 13.72904124 + z: 10.2126987 +- x: 3.5724037 + y: 20.29029381 + z: 10.63665947 +- x: 7.49702381 + y: 21.41133315 + z: 2.32287156 +- x: 7.91191336 + y: 18.23690022 + z: 1.65530874 +- x: 8.01628097 + y: 15.03729215 + z: 0.98243386 +- x: 7.95638556 + y: 11.78169855 + z: 0.8304708 +- x: 7.90871925 + y: 8.51014555 + z: 0.83046985 +- x: 8.00305533 + y: 4.69426217 + z: 0.83047505 +- x: 8.18310978 + y: 1.42714225 + z: 0.8304852 +- x: 8.39343911 + y: -1.83824164 + z: 0.83051365 +- x: 8.48951463 + y: -1.83844166 + z: 5.16629332 +- x: 7.59496851 + y: 11.08610671 + z: 4.65554922 +- x: 7.70174458 + y: 4.37108182 + z: 4.67018384 +- x: 8.14369098 + y: 1.28202463 + z: 4.96284079 +- x: 7.45041044 + y: 7.98683531 + z: 4.41370589 +- x: 7.94076231 + y: 17.55570873 + z: 5.00276188 +- x: 7.81258117 + y: 14.21466797 + z: 5.08267734 +- x: 7.61104993 + y: 20.74950266 + z: 5.49770801 +- x: -4.03569563 + y: 28.08244307 + z: 3.72577291 +- x: -6.62767274 + y: 24.49442143 + z: 2.9712037 +- x: -6.53635781 + y: 24.33135486 + z: 5.83911297 +- x: -4.09826233 + y: 27.00584015 + z: 7.05315498 +- x: -5.72524942 + y: 23.13913822 + z: 8.34520194 +- x: -3.13511549 + y: 23.80404808 + z: 10.10288971 +- x: -7.38872988 + y: -1.83843131 + z: 9.31075593 +- x: -4.25469072 + y: -1.83842924 + z: 12.2044915 +- x: -6.37805738 + y: 20.12836446 + z: 8.46599775 +- x: -6.19975695 + y: 16.80815911 + z: 8.75863336 +- x: -6.09392303 + y: 13.78451673 + z: 8.28088525 +- x: -5.81396004 + y: 10.82948586 + z: 7.59170343 +- x: -5.67589675 + y: 7.37239728 + z: 7.04781175 +- x: -5.6229397 + y: 4.30120402 + z: 7.98710121 +- x: -6.51658251 + y: 1.00320616 + z: 8.81523868 +- x: -2.90784487 + y: 10.8145338 + z: 9.36445453 +- x: -3.1353787 + y: 4.13280498 + z: 9.48359329 +- x: -3.59420968 + y: 1.33607952 + z: 10.72327448 +- x: -2.85383295 + y: 7.20978104 + z: 8.63989556 +- x: -3.69669249 + y: 17.22514737 + z: 10.57684617 +- x: -3.05173451 + y: 13.72904124 + z: 10.2126987 +- x: -3.5724037 + y: 20.29029381 + z: 10.63665947 +- x: -7.49702381 + y: 21.41133315 + z: 2.32287156 +- x: -7.91191336 + y: 18.23690022 + z: 1.65530874 +- x: -8.01628097 + y: 15.03729215 + z: 0.98243386 +- x: -7.95638556 + y: 11.78169855 + z: 0.8304708 +- x: -7.90871925 + y: 8.51014555 + z: 0.83046985 +- x: -8.00305533 + y: 4.69426217 + z: 0.83047505 +- x: -8.18310978 + y: 1.42714225 + z: 0.8304852 +- x: -8.39343911 + y: -1.83824164 + z: 0.83051365 +- x: -8.48951463 + y: -1.83844166 + z: 5.16629332 +- x: -7.59496851 + y: 11.08610671 + z: 4.65554922 +- x: -7.70174458 + y: 4.37108182 + z: 4.67018384 +- x: -8.14369098 + y: 1.28202463 + z: 4.96284079 +- x: -7.45041044 + y: 7.98683531 + z: 4.41370589 +- x: -7.94076231 + y: 17.55570873 + z: 5.00276188 +- x: -7.81258117 + y: 14.21466797 + z: 5.08267734 +- x: -7.61104993 + y: 20.74950266 + z: 5.49770801 diff --git a/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml new file mode 100644 index 00000000..f4212dff --- /dev/null +++ b/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml @@ -0,0 +1,157 @@ +name: touch-sensor-pinky +description: ORCA Fingertip - Pinky +num_taxels: 51 +coordinates: +- x: -7.31378277 + y: -1.79988568 + z: 0.39999828 +- x: -7.35349939 + y: -1.79999907 + z: 3.44426423 +- x: -7.14890952 + y: 2.23310916 + z: 0.39995294 +- x: -7.239463 + y: 1.5965166 + z: 3.25227094 +- x: -6.96664673 + y: 5.76010809 + z: 0.39988187 +- x: -7.06807114 + y: 5.49744394 + z: 3.03648049 +- x: -6.58421581 + y: 9.77742185 + z: 0.39989891 +- x: -6.00996443 + y: -1.79999958 + z: 6.67266775 +- x: -6.16505672 + y: 1.39784473 + z: 6.53131412 +- x: -6.19950575 + y: 5.14650534 + z: 6.31004181 +- x: -6.80097742 + y: 8.9453581 + z: 2.83870363 +- x: -5.89906718 + y: 8.44673336 + z: 5.91034299 +- x: -5.71308078 + y: 13.71144591 + z: 0.40007939 +- x: -5.91467725 + y: 12.89776522 + z: 2.62718238 +- x: -3.03222976 + y: -1.79999629 + z: 8.50433192 +- x: -3.21085316 + y: 2.11997188 + z: 8.71002527 +- x: -3.71209236 + y: 6.06174158 + z: 8.34183848 +- x: -5.16324911 + y: 11.6529258 + z: 5.37155488 +- x: -3.34056505 + y: 9.91752203 + z: 7.56053006 +- x: -3.73628217 + y: 16.5905664 + z: 0.3999584 +- x: -3.45158551 + y: 15.76622462 + z: 3.67837322 +- x: -2.76185013 + y: 13.00866685 + z: 6.43839714 +- x: -1.0e-08 + y: -1.8 + z: 8.66869815 +- x: -1.0e-08 + y: 2.14667381 + z: 8.97522434 +- x: -1.0e-08 + y: 6.09991603 + z: 8.84761706 +- x: -1.0e-08 + y: 9.96543874 + z: 8.00437736 +- x: -1.0e-08 + y: 13.63052494 + z: 6.51654897 +- x: -1.0e-08 + y: 16.73346681 + z: 4.10241064 +- x: -1.0e-08 + y: 17.90374908 + z: 0.39997619 +- x: 2.76185013 + y: 13.00866685 + z: 6.43839714 +- x: 3.03222976 + y: -1.79999629 + z: 8.50433192 +- x: 3.71209236 + y: 6.06174158 + z: 8.34183848 +- x: 3.34056505 + y: 9.91752203 + z: 7.56053006 +- x: 3.45158551 + y: 15.76622462 + z: 3.67837322 +- x: 3.21085316 + y: 2.11997188 + z: 8.71002527 +- x: 3.73628217 + y: 16.5905664 + z: 0.3999584 +- x: 5.16324911 + y: 11.6529258 + z: 5.37155488 +- x: 6.00996443 + y: -1.79999958 + z: 6.67266775 +- x: 6.16505672 + y: 1.39784473 + z: 6.53131412 +- x: 6.19950575 + y: 5.14650534 + z: 6.31004181 +- x: 5.89906718 + y: 8.44673336 + z: 5.91034299 +- x: 5.91467725 + y: 12.89776522 + z: 2.62718238 +- x: 7.35349939 + y: -1.79999907 + z: 3.44426423 +- x: 7.31378277 + y: -1.79988568 + z: 0.39999828 +- x: 7.239463 + y: 1.5965166 + z: 3.25227094 +- x: 7.14890952 + y: 2.23310916 + z: 0.39995294 +- x: 7.06807114 + y: 5.49744394 + z: 3.03648049 +- x: 6.96664673 + y: 5.76010809 + z: 0.39988187 +- x: 6.58421581 + y: 9.77742185 + z: 0.39989891 +- x: 6.80097742 + y: 8.9453581 + z: 2.83870363 +- x: 5.71308078 + y: 13.71144591 + z: 0.40007939 diff --git a/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml new file mode 100644 index 00000000..ede3ce05 --- /dev/null +++ b/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml @@ -0,0 +1,157 @@ +name: touch-sensor-thumb +description: ORCA Fingertip - Thumb +num_taxels: 51 +coordinates: +- x: -9.98783684 + y: -0.99999966 + z: 0.83664511 +- x: -9.22014693 + y: -0.60386497 + z: 4.2141522 +- x: -9.98563473 + y: 3.01522059 + z: 0.83825563 +- x: -9.34725868 + y: 3.12493816 + z: 3.96201581 +- x: -9.86319537 + y: 7.025051 + z: 0.83668072 +- x: -9.13066251 + y: 6.90457409 + z: 4.21467613 +- x: -8.90947253 + y: 10.91402385 + z: 0.83668825 +- x: -7.08587129 + y: -0.36341653 + z: 6.95899868 +- x: -6.93554584 + y: 2.94656029 + z: 7.10574445 +- x: -7.07510324 + y: 6.26486515 + z: 6.97833064 +- x: -8.34358177 + y: 10.63405643 + z: 3.83946027 +- x: -6.78658109 + y: 9.61401604 + z: 6.5180933 +- x: -7.01397155 + y: 14.44094726 + z: 0.83664566 +- x: -6.67742466 + y: 13.86389925 + z: 3.48169799 +- x: -3.4990712 + y: -0.12231963 + z: 8.65774898 +- x: -3.66401984 + y: 3.40154211 + z: 8.6618383 +- x: -3.71268789 + y: 6.92448678 + z: 8.61804962 +- x: -5.73110824 + y: 12.7621635 + z: 5.78836754 +- x: -3.55144846 + y: 10.41168292 + z: 8.07657784 +- x: -3.93225521 + y: 16.92530102 + z: 0.83665819 +- x: -3.38716757 + y: 15.99481755 + z: 4.32509838 +- x: -2.89494885 + y: 14.00354271 + z: 6.63864407 +- x: 0.00021972 + y: 0.01158422 + z: 8.79792926 +- x: 0.00015816 + y: 3.55089765 + z: 8.83782324 +- x: 9.735e-05 + y: 7.09004225 + z: 8.82407653 +- x: 3.779e-05 + y: 10.59777145 + z: 8.38341654 +- x: 0.0 + y: 14.30397222 + z: 6.83198881 +- x: 0.0 + y: 16.63586847 + z: 4.20815607 +- x: 0.0 + y: 17.61962965 + z: 0.83666099 +- x: 2.89494885 + y: 14.00354271 + z: 6.63864407 +- x: 3.4990712 + y: -0.12231963 + z: 8.65774898 +- x: 3.71268789 + y: 6.92448678 + z: 8.61804962 +- x: 3.55144846 + y: 10.41168292 + z: 8.07657784 +- x: 3.38716757 + y: 15.99481755 + z: 4.32509838 +- x: 3.66401984 + y: 3.40154211 + z: 8.6618383 +- x: 3.93225521 + y: 16.92530102 + z: 0.83665819 +- x: 5.73110824 + y: 12.7621635 + z: 5.78836754 +- x: 7.08587129 + y: -0.36341653 + z: 6.95899868 +- x: 6.93554584 + y: 2.94656029 + z: 7.10574445 +- x: 7.07510324 + y: 6.26486515 + z: 6.97833064 +- x: 6.78658109 + y: 9.61401604 + z: 6.5180933 +- x: 6.67742466 + y: 13.86389925 + z: 3.48169799 +- x: 9.22014693 + y: -0.60386497 + z: 4.2141522 +- x: 9.98783684 + y: -0.99999966 + z: 0.83664511 +- x: 9.34725868 + y: 3.12493816 + z: 3.96201581 +- x: 9.98563473 + y: 3.01522059 + z: 0.83825563 +- x: 9.13066251 + y: 6.90457409 + z: 4.21467613 +- x: 9.86319537 + y: 7.025051 + z: 0.83668072 +- x: 8.90947253 + y: 10.91402385 + z: 0.83668825 +- x: 8.34358177 + y: 10.63405643 + z: 3.83946027 +- x: 7.01397155 + y: 14.44094726 + z: 0.83664566 diff --git a/orca_core/hardware/sensing/sensor_client.py b/orca_core/hardware/sensing/sensor_client.py new file mode 100644 index 00000000..60c583f0 --- /dev/null +++ b/orca_core/hardware/sensing/sensor_client.py @@ -0,0 +1,1636 @@ +# ============================================================================== +# 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 dataclasses import dataclass, field +from typing import Optional +import serial +import threading +import time +import logging + +# Configure logging +logger = logging.getLogger(__name__) + +FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] + +# Exceptions +class NoSensorsAvailableError(Exception): + """Raised when no sensors are available for communication.""" + pass + + +# Protocol constants +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 +ADDR_HARDWARE_VERSION_START = 0x0000 +ADDR_HARDWARE_VERSION_LENGTH = 16 + +ADDR_RESET = 0x0022 + +ADDR_CONNECTED_SENSORS_START = 0x0010 +ADDR_CONNECTED_SENSORS_LENGTH = 4 + +ADDR_NUM_TAXELS_START = 0x0030 +ADDR_NUM_TAXELS_LENGTH = 56 + +ADDR_RESULTING_FORCE_START = 0x0500 +ADDR_RESULTING_FORCE_LENGTH = 168 + +ADDR_AUTO_DATA_TYPE = 0x0016 +ADDR_AUTO_ENABLE = 0x0017 + + +def calculate_checksum(frame: bytes) -> int: + """Calculate checksum for the protocol frame. + + Algorithm: LRC + + Args: + frame: The frame bytes to calculate checksum for (excluding checksum byte) + + Returns: + Checksum value (single byte) + """ + total_sum = sum(frame) + lower_8_bits = total_sum & 0xFF + checksum = (0x100 - lower_8_bits) & 0xFF + + return checksum + +def int_to_little_endian(value: int, num_bytes: int = 2) -> bytes: + """Convert integer to little-endian bytes. + + Args: + value: Integer value to convert + num_bytes: Number of bytes to use (default: 2) + + Returns: + Little-endian byte representation + """ + return value.to_bytes(num_bytes, byteorder='little') + +@dataclass +class AutoStreamStats: + frames_ok: int = 0 + frames_bad_lrc: int = 0 + resyncs: int = 0 + last_error_code: int = 0 + parse_ok: int = 0 + parse_errors: int = 0 + last_eff_len: int = 0 + last_payload_len: int = 0 + consecutive_errors: int = 0 # For error-triggered reconfiguration + reconfiguration_count: int = 0 # Number of times config was updated + + +@dataclass +class SensorConfiguration: + """Snapshot of connected sensors and their properties. + + This configuration is captured when connecting or when errors trigger + reconfiguration. It's used to build dynamic parsers that adapt to + available sensors. + """ + 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} + expected_payload_size_resultant: int = 0 # Expected bytes for resultant force mode + expected_payload_size_taxels: int = 0 # Expected bytes for taxel mode + expected_payload_size_combined: int = 0 # Expected bytes for resultant + taxel mode + timestamp: float = 0.0 # When this config was captured + finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: { + "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 + }) + + @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. + """ + 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: + """Number of currently connected sensors.""" + return len(self.active_sensors) + + def __str__(self) -> str: + """Human-readable representation.""" + active = ", ".join(self.active_sensors) if self.active_sensors else "none" + return f"SensorConfig({self.num_active_sensors} active: {active})" + + +class SensorClient: + """Client for communicating with ORCA Tactile Sensors""" + + def __init__(self, + port: str = '/dev/ttyUSB0', + baudrate: int = 921600, + finger_to_sensor_id: Optional[dict[str, int]] = None): + + self.port = port + self.baudrate = baudrate + self._connected = False + self._serial_connection: Optional[serial.Serial] = None + + # Finger-to-sensor-id mapping (configurable wiring) + if finger_to_sensor_id is None: + self._finger_to_sensor_id = { + "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 + } + 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()} + + # Sensor configuration (dynamic, adapts to connected sensors) + self._sensor_config: Optional[SensorConfiguration] = None + self._last_reconfigure_time: float = 0.0 # Rate limiting for reconfiguration + + self._auto_thread: Optional[threading.Thread] = None + self._auto_running = threading.Event() # Thread-safe flag for auto stream + self._auto_lock = threading.Lock() + self._auto_latest = None # parsed resultant forces dict + self._auto_latest_taxels = None # parsed taxels dict + self._auto_latest_ts = None + self._auto_stats = AutoStreamStats() + self._auto_mode_resultant = True # Whether to parse resultant forces + self._auto_mode_taxels = False # Whether to parse taxels + + # Per-taxel zeroing offsets + self._taxel_offsets: Optional[dict] = None # {finger: [[fx, fy, fz], ...], ...} + self._resultant_offsets: Optional[dict] = None # {finger: [fx, fy, fz], ...} + + @property + def is_connected(self) -> bool: + """Check if client is connected.""" + return self._connected + + def connect(self): + """Connect to the sensor device and get initial configuration. + + This method establishes serial communication and reads the initial sensor + configuration (which sensors are connected, taxel counts, etc.). + + Raises: + ConnectionError: If serial connection fails + IOError: If unable to read 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}") + + # Get initial sensor configuration + try: + self._sensor_config = self._get_configuration() + logger.info(f"Initial configuration: {self._sensor_config}") + except Exception as e: + logger.warning(f"Failed to get initial configuration: {e}") + # Don't fail connection, config will be retrieved when starting auto-stream + + except Exception as e: + raise ConnectionError(f"Failed to connect to sensor at {self.port}: {e}") from e + + def disconnect(self): + """Disconnect from the sensor device.""" + 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: + """Read one or more registers + + Protocol flow: + 1. Send request frame: 55 AA | reserved | 0x03 | addr(2) | count(2) | LRC + 2. Wait for response frame: AA 55 | reserved | 0x03 | addr(2) | count(2) | data(count) | LRC + 3. Handle auto frames (AA 56) that may arrive while waiting for response + + This method is robust against auto-stream mode: while waiting for the AA55 + response, any AA56 auto frames that arrive are skipped automatically. + + Args: + address: Register address to read from + count: Number of bytes to read + response_timeout_s: Maximum time to wait for response (default: 0.5s) + + Returns: + Raw bytes read from registers + + Raises: + OSError: If not connected + TimeoutError: If no response received within timeout + IOError: If response checksum fails + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + # Build request frame: 55 AA | reserved | func(0x03=READ) | addr | count | LRC + request = ( + PROTOCOL_HEADER_REQUEST # 55 AA + + int_to_little_endian(PROTOCOL_RESERVED, 1) # 0x00 + + int_to_little_endian(FUNC_CODE_READ, 1) # 0x03 + + int_to_little_endian(address, 2) # Address (little-endian) + + int_to_little_endian(count, 2) # Byte count (little-endian) + ) + request += bytes([calculate_checksum(request)]) + + # Clear stale data if not streaming (prevents reading old responses) + if not self._is_streaming(): + self._serial_connection.reset_input_buffer() + + self._serial_connection.write(request) + + # Wait for AA55 response header, skipping any AA56 auto frames + 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: # AA56 auto frame + self._skip_auto_frame() + continue + break # Found AA55 response + + # Parse response: AA 55 | meta(6) | data(count) | LRC(1) + meta = self._read_exact(6) # reserved(1) + func(1) + addr(2) + count(2) + data = self._read_exact(count) + lrc = self._read_exact(1) + + # Validate checksum + full = hdr + meta + data + lrc + if full[-1] != calculate_checksum(full[:-1]): + raise IOError("Read response LRC mismatch") + + return data + + def _skip_auto_frame(self) -> None: + """Skip one complete auto-stream frame after having consumed the AA56 header. + + Frame format (after AA56 header): + - reserved (1 byte): typically 0x00 + - eff_len (2 bytes, little-endian): length of error_code + payload + - payload (eff_len bytes): error_code(1) + valid_data(eff_len-1) + - LRC (1 byte): checksum + + This is called when waiting for a request-response (AA55) frame but an + auto-stream (AA56) frame arrives first. We skip it to continue waiting + for the AA55 response. + + Raises: + IOError: If serial read fails + ValueError: If eff_len is unreasonably large (>8KB) + """ + _reserved = self._read_exact(1) + eff_len = int.from_bytes(self._read_exact(2), "little") + + # Sanity check: typical payload is 6-200 bytes, max reasonable is ~8KB + if eff_len > 8192: + raise ValueError(f"Invalid eff_len in auto frame: {eff_len} (possible corruption)") + + # Read and discard payload + LRC + _ = self._read_exact(eff_len + 1) + + def _read_header_resync(self, timeout_s: float) -> bytes: + """Read bytes until we find either AA55 (response) or AA56 (auto) header. + + Uses a sliding 2-byte window to locate frame headers even if the byte + stream starts mid-frame or is misaligned. This is critical for robustness + when auto-stream frames (AA56) can arrive at any time, even when waiting + for request-response frames (AA55). + + Args: + timeout_s: Maximum time to search for a header before giving up + + Returns: + The 2-byte header (either AA55 or AA56) + + Raises: + TimeoutError: If no valid header found within timeout_s + """ + deadline = time.time() + timeout_s + + # Sliding 2-byte window: [b1, b] + b1 = b"" + while time.time() < deadline: + b = self._serial_connection.read(1) + if not b: + continue # Serial timeout tick, keep trying until deadline + + # Build up 2-byte window + if not b1: + b1 = b + continue + + hdr = b1 + b + # Check if we found a valid header + if hdr == PROTOCOL_HEADER_RESPONSE or hdr == PROTOCOL_HEADER_AUTO: + return hdr + + # Slide window: b becomes new b1 + 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: + """Write bytes to registers + + Protocol flow: + 1. Send request: 55 AA | reserved | 0x10 | addr(2) | len(2) | data | LRC + 2. Wait for response: AA 55 | reserved | 0x10 | addr(2) | len(2) | status | LRC + 3. Handle auto frames (AA 56) that may arrive while waiting for response + 4. Check status byte (0x00 = success) + + This method is robust against auto-stream mode: while waiting for the AA55 + response, any AA56 auto frames that arrive are skipped automatically. + + Args: + address: Register address to write to + data: Bytes to write (max 10 bytes according to manual) + response_timeout_s: Maximum time to wait for response (default: 0.5s) + + Raises: + OSError: If not connected + TimeoutError: If no response received within timeout + IOError: If response checksum fails or status byte indicates failure + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + # Build request frame: 55 AA | reserved | func(0x10=WRITE) | addr | len | data | LRC + request = ( + PROTOCOL_HEADER_REQUEST # 55 AA + + int_to_little_endian(PROTOCOL_RESERVED, 1) # 0x00 + + int_to_little_endian(FUNC_CODE_WRITE, 1) # 0x10 + + int_to_little_endian(address, 2) # Address (little-endian) + + int_to_little_endian(len(data), 2) # Data length (little-endian) + + data # Payload + ) + request += bytes([calculate_checksum(request)]) + + # Clear stale data if not streaming (prevents reading old responses) + if not self._is_streaming(): + self._serial_connection.reset_input_buffer() + + self._serial_connection.write(request) + + # Wait for AA55 response header, skipping any AA56 auto frames + 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: # AA56 auto frame + self._skip_auto_frame() + continue + + # Found AA55 response + break + + # Parse response: AA 55 | meta(6) | status(1) | LRC(1) + # Meta fields: reserved(1) + func(1) + addr(2) + nbytes(2) + fixed_rest = self._read_exact(6) + fixed = hdr + fixed_rest # Total 8 bytes + + returned_nbytes = int.from_bytes(fixed[6:8], "little") + + # Read status + LRC + rest = self._read_exact(returned_nbytes + 1) + full = fixed + rest + + # Validate checksum + expected = calculate_checksum(full[:-1]) + if full[-1] != expected: + raise IOError("Write response LRC mismatch") + + # Check status byte (first byte of response payload) + if returned_nbytes >= 1: + status = rest[0] + if status != 0: + raise IOError(f"Write failed, status=0x{status:02X}") + + + + def read_connected_sensors(self) -> dict[str, bool]: + """Read the connected sensors. + + Returns: + Dictionary of sensor names and their status + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + data = self._read_register(ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH) + if len(data) < 4: + raise ValueError(f'Expected 4 bytes, got {len(data)}') + status = [ + bool(data[0] & (1 << 2)), + bool(data[0] & (1 << 6)), + bool(data[1] & (1 << 2)), + bool(data[1] & (1 << 6)), + bool(data[2] & (1 << 2)), + ] + return {self._sensor_id_to_finger[i]: status[i] for i in range(5)} + + def read_hardware_version(self) -> str: + """Read the hardware version. + + Returns: + Hardware version string + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + version_bytes = self._read_register(ADDR_HARDWARE_VERSION_START, ADDR_HARDWARE_VERSION_LENGTH) + version_string = version_bytes.decode('ascii', errors='ignore').rstrip('\x00').rstrip() + + return version_string + + def read_num_taxels(self) -> dict[str, int]: + """Read the number of taxels for each fingertip sensor. + + Returns: + Dictionary mapping finger names to taxel counts + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + data = self._read_register(ADDR_NUM_TAXELS_START, ADDR_NUM_TAXELS_LENGTH) + taxel_counts = [int.from_bytes(data[i:i+2], byteorder='little') for i in range(0, len(data), 2)] + SLOT_REGISTER_OFFSETS = [0x0034, 0x003C, 0x0044, 0x004C, 0x0054] + distal_indices = { + self._sensor_id_to_finger[slot]: (addr - ADDR_NUM_TAXELS_START) // 2 + for slot, addr in enumerate(SLOT_REGISTER_OFFSETS) + } + return {finger: taxel_counts[idx] for finger, idx in distal_indices.items()} + + def read_auto_data_type(self) -> dict: + """Read the auto data type register. + + Returns: + Dictionary with raw binary value and parsed flags + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + data = self._read_register(ADDR_AUTO_DATA_TYPE, 1) + byte_val = data[0] + return { + "raw": f"{byte_val:08b}", + "resulting_force": bool(byte_val & 0x01), + "individual_taxels_force": bool(byte_val & 0x02), + } + + def read_resulting_force(self) -> dict[str, list[float]]: + """Read resulting force from all connected fingertip sensors. + + Uses dynamic parsing based on current sensor configuration. Returns data + only for sensors that are currently connected (sparse dict format). + + Note: This implementation assumes we only have fingertip (distal phalanx) sensors + for each finger, not proximal/middle phalanx or palm sensors. + + Returns: + Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons + Only includes sensors that are currently connected + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + # Ensure we have current configuration + if self._sensor_config is None: + try: + self._sensor_config = self._get_configuration() + except Exception as e: + logger.error(f"Failed to get configuration: {e}") + # Fall back to static parsing + data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) + return self._parse_resultant_force_block(data) + + # Use dynamic parsing based on configuration + data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) + result = self._parse_resultant_force_dynamic(data, self._sensor_config) + if self._resultant_offsets: + self._apply_resultant_offsets(result) + return result + + def get_sensor_configuration(self) -> Optional[SensorConfiguration]: + """Get the current sensor configuration snapshot. + + Returns: + SensorConfiguration object with current sensor state, or None if not yet configured + """ + return self._sensor_config + + def _get_configuration(self) -> SensorConfiguration: + """Snapshot the current sensor configuration. + + Reads connected sensors and their properties from the hardware. + This is called on connect and when errors trigger reconfiguration. + + Returns: + SensorConfiguration with current hardware state + + Raises: + IOError: If unable to read configuration from sensor + """ + try: + connected = self.read_connected_sensors() + num_taxels = self.read_num_taxels() + + # Build module indices for active sensors (fingertip only) + module_indices = {} + for finger in FINGER_NAMES: + if connected.get(finger, False): + sensor_id = self._finger_to_sensor_id[finger] + module_indices[finger] = sensor_id * 4 + 2 + + # Calculate expected payload sizes + num_active = sum(1 for c in connected.values() if c) + expected_resultant = num_active * 6 # Each sensor: fx(2) + fy(2) + fz(2) = 6 bytes + + # Calculate taxel payload size: sum of taxels for active sensors * 3 bytes per taxel + # Each taxel sends 3 bytes: fx(1) + fy(1) + fz(1) as int8 values + expected_taxels = sum( + num_taxels.get(finger, 0) * 3 + for finger, is_connected in connected.items() + if is_connected + ) + + # Combined mode: resultant forces followed by taxels + expected_combined = expected_resultant + expected_taxels + + config = SensorConfiguration( + connected=connected, + num_taxels=num_taxels, + module_indices=module_indices, + expected_payload_size_resultant=expected_resultant, + expected_payload_size_taxels=expected_taxels, + expected_payload_size_combined=expected_combined, + timestamp=time.time(), + finger_to_sensor_id=dict(self._finger_to_sensor_id), + ) + + logger.info(f"Configuration captured: {config}") + return config + + except Exception as e: + logger.error(f"Failed to get sensor configuration: {e}") + raise IOError(f"Failed to read sensor configuration: {e}") from e + + def _reconfigure(self, force: bool = False) -> bool: + """Attempt to reconfigure sensor client with current hardware state. + + This is called when errors indicate the sensor configuration may have changed + (e.g., sensor disconnected or reconnected). It rate-limits reconfiguration + to avoid thrashing on flaky connections. + + Args: + force: If True, bypass rate limiting and reconfigure immediately + + Returns: + True if reconfiguration succeeded and configuration changed, False otherwise + + Raises: + NoSensorsAvailableError: If no sensors are connected after reconfiguration + """ + # Rate limiting: don't reconfigure more than once per 2 seconds (unless forced) + now = time.time() + if not force and (now - self._last_reconfigure_time) < 2.0: + logger.debug("Reconfiguration rate-limited, skipping") + return False + + try: + logger.info("Attempting reconfiguration...") + new_config = self._get_configuration() + + # Check if configuration actually changed + if self._sensor_config is not None: + old_active = set(self._sensor_config.active_sensors) + new_active = set(new_config.active_sensors) + + if old_active == new_active: + logger.debug("Configuration unchanged, no reconfiguration needed") + return False + + # Log configuration changes + added = new_active - old_active + removed = old_active - new_active + if added: + logger.info(f"Sensors added: {', '.join(added)}") + if removed: + logger.warning(f"Sensors removed: {', '.join(removed)}") + + # Update configuration + self._sensor_config = new_config + self._last_reconfigure_time = now + + with self._auto_lock: + self._auto_stats.reconfiguration_count += 1 + self._auto_stats.consecutive_errors = 0 # Reset error counter + + # Check if we have any sensors left + if new_config.num_active_sensors == 0: + logger.error("No sensors available after reconfiguration") + raise NoSensorsAvailableError("All sensors disconnected") + + logger.info(f"Reconfiguration successful: {new_config}") + return True + + except Exception as e: + logger.error(f"Reconfiguration failed: {e}") + raise + + def _parse_auto_stream_compact(self, data: bytes, config: SensorConfiguration) -> dict[str, list[float]]: + """Parse auto-stream compact format (active sensors only, sequential). + + Auto-stream mode sends only data for connected sensors in sequential order. + Each sensor is 6 bytes: fx(2) + fy(2) + fz(2). + + For example, if only thumb and middle are connected: + - Bytes 0-5: thumb (fx, fy, fz) + - Bytes 6-11: middle (fx, fy, fz) + + Args: + data: Raw byte data from auto-stream (6 * num_active_sensors bytes) + config: Current sensor configuration + + Returns: + Dictionary mapping active finger names to [fx, fy, fz] force vectors + Uses sparse dict format (only active sensors included) + + Raises: + ValueError: If data size doesn't match expected size + """ + RESOLUTION_N_PER_LSB = 0.1 + BYTES_PER_SENSOR = 6 + + expected_size = config.num_active_sensors * BYTES_PER_SENSOR + if len(data) != expected_size: + raise ValueError( + f"Auto-stream compact data size mismatch: " + f"expected {expected_size} bytes ({config.num_active_sensors} sensors), " + f"got {len(data)} bytes" + ) + + result = {} + for i, finger in enumerate(config.active_sensors): + offset = i * BYTES_PER_SENSOR + + # Parse force data sequentially + fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB + + result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] + + return result + + def _parse_taxels_compact(self, data: bytes, config: SensorConfiguration) -> dict[str, list[list[float]]]: + """Parse auto-stream taxels-only format (active sensors only, sequential). + + Auto-stream taxels mode sends only taxel data for connected sensors in sequential order. + Each taxel is 3 bytes: fx(int8) + fy(int8) + fz(int8). + + For example, if thumb (127 taxels) and index (52 taxels) are connected: + - Bytes 0-380: thumb taxels (127 * 3 = 381 bytes) + - Bytes 381-536: index taxels (52 * 3 = 156 bytes) + + Args: + data: Raw byte data from auto-stream + config: Current sensor configuration + + Returns: + Dictionary mapping active finger names to list of taxel force vectors [fx, fy, fz] + + Raises: + ValueError: If data size doesn't match expected size + """ + RESOLUTION_N_PER_LSB = 0.1 + BYTES_PER_TAXEL = 3 + + expected_size = config.expected_payload_size_taxels + if len(data) != expected_size: + raise ValueError( + f"Auto-stream taxels data size mismatch: " + f"expected {expected_size} bytes, got {len(data)} bytes" + ) + + result = {} + offset = 0 + for finger in config.active_sensors: + taxel_count = config.num_taxels.get(finger, 0) + taxels = [] + for _ in range(taxel_count): + # Each taxel: fx(int8), fy(int8), fz(uint8) + fx = int.from_bytes(data[offset:offset+1], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fy = int.from_bytes(data[offset+1:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fz = int.from_bytes(data[offset+2:offset+3], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB + taxels.append([round(fx, 2), round(fy, 2), round(fz, 2)]) + offset += BYTES_PER_TAXEL + result[finger] = taxels + + return result + + def _parse_combined_compact( + self, data: bytes, config: SensorConfiguration + ) -> tuple[dict[str, list[float]], dict[str, list[list[float]]]]: + """Parse auto-stream combined format (resultant + taxels for active sensors). + + Combined mode sends data interleaved per sensor: + [sensor1_resultant][sensor1_taxels][sensor2_resultant][sensor2_taxels]... + + Args: + data: Raw byte data from auto-stream + config: Current sensor configuration + + Returns: + Tuple of (resultant_forces, taxels): + - resultant_forces: Dict mapping finger names to [fx, fy, fz] + - taxels: Dict mapping finger names to list of taxel values + + Raises: + ValueError: If data size doesn't match expected size + """ + BYTES_PER_RESULTANT = 6 + BYTES_PER_TAXEL = 3 + RESOLUTION_RESULTANT = 0.1 + RESOLUTION_TAXEL = 0.1 + + expected_size = config.expected_payload_size_combined + if len(data) != expected_size: + raise ValueError( + f"Auto-stream combined data size mismatch: " + f"expected {expected_size} bytes, got {len(data)} bytes" + ) + + offset = 0 + resultant_forces = {} + taxels = {} + + for finger in config.active_sensors: + # Parse resultant (6 bytes: fx:int16, fy:int16, fz:uint16) + fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_RESULTANT + fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_RESULTANT + fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_RESULTANT + resultant_forces[finger] = [round(fx, 1), round(fy, 1), round(fz, 1)] + offset += BYTES_PER_RESULTANT + + # Parse taxels (taxel_count × 3 bytes: fx:int8, fy:int8, fz:uint8) + taxel_count = config.num_taxels.get(finger, 0) + finger_taxels = [] + for _ in range(taxel_count): + tfx = int.from_bytes(data[offset:offset+1], byteorder='little', signed=True) * RESOLUTION_TAXEL + tfy = int.from_bytes(data[offset+1:offset+2], byteorder='little', signed=True) * RESOLUTION_TAXEL + tfz = int.from_bytes(data[offset+2:offset+3], byteorder='little', signed=False) * RESOLUTION_TAXEL + finger_taxels.append([round(tfx, 2), round(tfy, 2), round(tfz, 2)]) + offset += BYTES_PER_TAXEL + taxels[finger] = finger_taxels + + return resultant_forces, taxels + + def _parse_resultant_force_dynamic(self, data: bytes, config: SensorConfiguration) -> dict[str, list[float]]: + """Parse resultant force data using dynamic configuration (offset-based). + + This parser is used for request-response mode where the full 168-byte block + is returned with all 28 modules. It adapts to the actual connected sensors, + returning data only for available sensors. Uses sparse dict format. + + Args: + data: Raw byte data from sensor (168 bytes for full block) + config: Current sensor configuration + + Returns: + Dictionary mapping active finger names to [fx, fy, fz] force vectors + Only includes sensors that are currently connected + + Raises: + ValueError: If data is too short for expected configuration + """ + RESOLUTION_N_PER_LSB = 0.1 + + if len(data) < ADDR_RESULTING_FORCE_LENGTH: + raise ValueError(f"Resultant force block too short: {len(data)} bytes") + + result = {} + for finger in config.active_sensors: + # Get module index for this sensor from configuration + module_idx = config.module_indices[finger] + offset = module_idx * 6 # Each module force is 6 bytes (fx, fy, fz) + + # Parse force data from fixed offsets in full block + fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB + + result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] + + return result + + + def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: + """Configure which data types to include in auto stream. + + Args: + resultant: Include resultant force data + taxels: Include individual taxel force data + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + val = (0x01 if resultant else 0) | (0x02 if taxels else 0) + self._write_register(ADDR_AUTO_DATA_TYPE, bytes([val])) + + + def enable_auto_data_transmission(self) -> None: + """Enable automatic data transmission mode. + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + self._write_register(ADDR_AUTO_ENABLE, bytes([0x01])) + + def disable_auto_data_transmission(self) -> None: + """Disable automatic data transmission mode. + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + self._write_register(ADDR_AUTO_ENABLE, bytes([0x00])) + + + def reboot(self) -> None: + """Reboot the sensor. + + Raises: + OSError: If not connected to sensor + """ + if not self.is_connected: + raise OSError("Must call connect() first.") + + self._write_register(ADDR_RESET, bytes([0x01])) + + def get_auto_latest(self): + """Get the most recently parsed auto-stream resultant force data (thread-safe). + + Returns a snapshot of the latest force data received from the auto-stream. + This is updated by the background thread at ~1kHz when auto streaming is active. + + Returns: + Tuple of (parsed_data, timestamp): + - parsed_data: Dictionary mapping finger names to [fx, fy, fz] forces (Newtons) + None if no data received yet or resultant mode not enabled + - timestamp: Unix timestamp (time.time()) when data was received + None if no data received yet + + Example: + >>> client.start_auto_stream(resultant=True) + >>> time.sleep(0.1) # Let some data arrive + >>> forces, ts = client.get_auto_latest() + >>> print(forces) + {'index': [0.1, -0.2, 1.5]} + """ + with self._auto_lock: + return self._auto_latest, self._auto_latest_ts + + def get_auto_latest_taxels(self): + """Get the most recently parsed auto-stream taxel data (thread-safe). + + Returns a snapshot of the latest taxel data received from the auto-stream. + Only available when auto-stream was started with taxels=True. + + Returns: + Tuple of (parsed_data, timestamp): + - parsed_data: Dictionary mapping finger names to list of taxel force vectors + Each taxel is [fx, fy, fz] in Newtons + None if no data received yet or taxel mode not enabled + - timestamp: Unix timestamp (time.time()) when data was received + None if no data received yet + + Example: + >>> client.start_auto_stream(resultant=False, taxels=True) + >>> time.sleep(0.1) + >>> taxels, ts = client.get_auto_latest_taxels() + >>> print(taxels) + {'index': [[0.1, -0.2, 0.5], [0.0, 0.1, 0.3], ...]} # list of [fx, fy, fz] per taxel + """ + with self._auto_lock: + return self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_latest_all(self): + """Get both resultant forces and taxels from latest auto-stream data (thread-safe). + + Returns all available data from the auto-stream. Useful when running in + combined mode (resultant=True, taxels=True). + + Returns: + Tuple of (resultant_forces, taxels, timestamp): + - resultant_forces: Dict mapping finger names to [fx, fy, fz], or None + - taxels: Dict mapping finger names to taxel value lists, or None + - timestamp: Unix timestamp when data was received, or None + + Example: + >>> client.start_auto_stream(resultant=True, taxels=True) + >>> time.sleep(0.1) + >>> forces, taxels, ts = client.get_auto_latest_all() + """ + with self._auto_lock: + return self._auto_latest, self._auto_latest_taxels, self._auto_latest_ts + + def get_auto_stats(self): + """Get auto-stream statistics (thread-safe). + + Returns diagnostic information about the auto-stream performance: + - frames_ok: Number of successfully received frames + - frames_bad_lrc: Number of frames with checksum errors + - resyncs: Number of times the reader had to resync after errors + - parse_ok: Number of successfully parsed frames + - parse_errors: Number of frames that couldn't be parsed + - last_error_code: Most recent error code from sensor (0 = no error) + + Returns: + AutoStreamStats dataclass with statistics + + Example: + >>> stats = client.get_auto_stats() + >>> print(f"Success rate: {stats.frames_ok}/{stats.frames_ok + stats.frames_bad_lrc}") + """ + with self._auto_lock: + return self._auto_stats + + def set_taxel_offsets(self, offsets: dict) -> None: + """Set per-taxel zeroing offsets and compute resultant offsets. + + Args: + offsets: {finger: [[fx, fy, fz], ...], ...} per-taxel 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 get_taxel_offsets(self) -> Optional[dict]: + """Return current per-taxel offsets (for saving to YAML).""" + return self._taxel_offsets + + def capture_taxel_offsets(self, num_samples: int = 100) -> dict: + """Capture live baseline offsets by averaging current sensor readings. + + Requires active auto-stream with taxels enabled. Temporarily clears + any existing offsets so raw sensor data is captured. + + Args: + num_samples: Number of unique frames to average + + Returns: + Per-taxel offsets dict: {finger: [[fx, fy, fz], ...], ...} + """ + if not self._is_streaming() or not self._auto_mode_taxels: + raise RuntimeError("Auto-stream with taxels must be active to capture offsets") + + # Temporarily clear offsets to capture raw data + prev_taxel = self._taxel_offsets + prev_resultant = self._resultant_offsets + self._taxel_offsets = None + self._resultant_offsets = None + + # Wait for at least one raw frame to flush old offset-applied data + time.sleep(0.01) + + try: + # Collect unique frames by checking timestamps + 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) + + # Average per-taxel [fx, fy, fz] across all frames + 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) + return offsets + except Exception: + # Restore previous offsets on failure + self._taxel_offsets = prev_taxel + self._resultant_offsets = prev_resultant + raise + + 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], 2) + taxel[1] = round(taxel[1] - off[1], 2) + taxel[2] = round(max(0, taxel[2] - off[2]), 2) + + 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 _read_exact(self, n: int) -> bytes: + """Read exactly n bytes from serial connection, blocking until complete. + + This is a fundamental building block for the protocol implementation. + Unlike serial.read(n) which may return fewer bytes, this guarantees + exactly n bytes are read or an error is raised. + + The serial connection has a 1.0s timeout (set in connect()). If no data + arrives within that window, this raises IOError. This prevents infinite + blocking on sensor disconnect or communication failure. + + Args: + n: Number of bytes to read + + Returns: + Exactly n bytes read from serial connection + + Raises: + IOError: If serial read times out (1.0s) or connection is closed + """ + out = bytearray() + while len(out) < n: + chunk = self._serial_connection.read(n - len(out)) + if chunk is None or len(chunk) == 0: + # Serial timeout or disconnect + 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: + """Scan byte stream until we see 0xAA 0x56 auto-stream header. + + Uses a sliding 2-byte window to find the header even if the stream + starts mid-frame or becomes misaligned. Exits cleanly when auto stream + is stopped via _auto_running.clear(). + + Performance note: Checking is_set() adds <0.01% overhead since serial I/O + (milliseconds) dominates over the flag check (microseconds). + + Raises: + IOError: If auto stream is stopped while resyncing + """ + # Sliding 2-byte window to find AA 56 header + b1 = self._read_exact(1) + while self._auto_running.is_set(): + b2 = self._read_exact(1) + if b1 == bytes([0xAA]) and b2 == bytes([0x56]): + return # Found the header + # Slide window: b2 becomes new b1 + b1 = b2 + + # If we exit the loop, auto stream was stopped + raise IOError("Auto stream stopped during resync") + + def _check_lrc(self, frame_wo_lrc: bytes, lrc_byte: int) -> bool: + expected = calculate_checksum(frame_wo_lrc) + return expected == lrc_byte + + + def _parse_resultant_force_block(self, data: bytes) -> dict[str, list[float]]: + """Parse a resultant force data block from the sensor. + + Module index calculation (module_idx = i * 4 + 2): + - Each finger has 4 potential modules: proximal(0), middle(1), distal(2), nail(3) + - We only use fingertip sensors, which are the distal phalanx (index 2) + - For thumb: module_idx = 0*4+2 = 2 (byte offset = 2*6 = 12) + - For index: module_idx = 1*4+2 = 6 (byte offset = 6*6 = 36) + - etc. + + Args: + data: Raw byte data containing resultant forces (168 bytes for 28 modules) + + Returns: + Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons + """ + RESOLUTION_N_PER_LSB = 0.1 + if len(data) < ADDR_RESULTING_FORCE_LENGTH: + raise ValueError(f"Resultant force block too short: {len(data)} bytes") + + result = {} + for finger in FINGER_NAMES: + sensor_id = self._finger_to_sensor_id[finger] + module_idx = sensor_id * 4 + 2 + offset = module_idx * 6 # Each module force is 6 bytes (fx, fy, fz) + + fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB + fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB + result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] + return result + + def _get_expected_payload_size(self, config: SensorConfiguration) -> int: + """Get expected payload size based on current streaming mode.""" + if self._auto_mode_resultant and self._auto_mode_taxels: + return config.expected_payload_size_combined + elif self._auto_mode_resultant: + return config.expected_payload_size_resultant + elif self._auto_mode_taxels: + return config.expected_payload_size_taxels + return 0 + + def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_sensors: int): + """Background thread that continuously reads and parses auto-stream frames. + + Auto-stream frame format (when enabled via 0x0017 = 1): + - Header: AA 56 (2 bytes) + - Reserved: 0x00 (1 byte) + - Effective length: eff_len (2 bytes, little-endian) + - Error code: (1 byte) - part of eff_len + - Valid data: (eff_len-1 bytes) - sensor force data + - LRC: (1 byte) - checksum + + The sensor continuously sends these frames at ~1kHz when auto mode is enabled. + This thread parses them and updates _auto_latest for the user to read via + get_auto_latest(). + + Supports three modes: + - Resultant only: 6 bytes per sensor (fx, fy, fz) + - Taxels only: 2 bytes per taxel for each sensor + - Combined: Resultant forces followed by taxels + + Features error-triggered reconfiguration: if consecutive errors exceed threshold, + attempts to reconfigure to adapt to changed sensor configuration. + + Args: + parse_resultant: Whether to parse resultant force data + parse_taxels: Whether to parse individual taxel data + min_sensors: Minimum number of sensors required to continue streaming + """ + last_print = 0.0 + ERROR_THRESHOLD = 5 # Trigger reconfiguration after 5 consecutive errors + + while self._auto_running.is_set(): + try: + # ===== Step 1: Find and consume AA 56 header ===== + self._resync_to_auto_header() + + # ===== Step 2: Read frame metadata ===== + reserved = self._read_exact(1) # Typically 0x00 + eff_len = int.from_bytes(self._read_exact(2), "little") + + # ===== Step 3: Read payload and checksum ===== + # Payload includes: error_code(1) + valid_data(eff_len-1) + payload = self._read_exact(eff_len) + lrc = self._read_exact(1)[0] + + # Debug print (throttled to once per second) + now = time.time() + if now - last_print > 1.0: + config_str = str(self._sensor_config) if self._sensor_config else "no config" + logger.debug(f"[auto] eff_len={eff_len}, config={config_str}") + last_print = now + + # ===== Step 4: Validate frame integrity ===== + frame_wo_lrc = bytes([0xAA, 0x56]) + reserved + int_to_little_endian(eff_len, 2) + payload + if not self._check_lrc(frame_wo_lrc, lrc): + with self._auto_lock: + self._auto_stats.frames_bad_lrc += 1 + self._auto_stats.consecutive_errors += 1 + continue # Skip corrupted frame, resync + + # ===== Step 5: Split error code and valid data ===== + err_code = payload[0] + valid = payload[1:] # The actual sensor data + + parsed_resultant = None + parsed_taxels = None + parse_success = False + + # ===== Step 6: Check for payload size mismatch (indicates config change) ===== + if self._sensor_config: + expected_size = self._get_expected_payload_size(self._sensor_config) + if len(valid) != expected_size and expected_size > 0: + # Payload size changed! Trigger immediate reconfiguration + logger.warning( + f"Payload size mismatch: expected {expected_size}, got {len(valid)}. " + "Triggering reconfiguration..." + ) + try: + if self._reconfigure(force=False): + logger.info("Reconfiguration successful, continuing stream") + continue # Skip this frame, let next iteration use new config + except NoSensorsAvailableError: + logger.error("No sensors available after reconfiguration, stopping stream") + self._auto_running.clear() + break + except Exception as e: + logger.error(f"Reconfiguration failed: {e}") + # Continue with old config + + # ===== Step 7: Parse payload based on mode ===== + if self._sensor_config: + try: + expected_size = self._get_expected_payload_size(self._sensor_config) + + if len(valid) == expected_size and expected_size > 0: + # Combined mode: resultant + taxels + if parse_resultant and parse_taxels: + parsed_resultant, parsed_taxels = self._parse_combined_compact( + valid, self._sensor_config + ) + parse_success = True + + # Resultant only mode + elif parse_resultant: + parsed_resultant = self._parse_auto_stream_compact( + valid, self._sensor_config + ) + parse_success = True + + # Taxels only mode + elif parse_taxels: + parsed_taxels = self._parse_taxels_compact( + valid, self._sensor_config + ) + parse_success = True + + else: + # Unexpected size + logger.warning( + f"Unexpected payload: {len(valid)} bytes, expected {expected_size}" + ) + parse_success = False + + except Exception as e: + logger.error(f"Parse error: {e}", exc_info=True) + parse_success = False + + # ===== Step 8: Apply zeroing offsets ===== + if parse_success: + 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) + + # ===== Step 9: Publish latest data and update stats ===== + with self._auto_lock: + if parse_success: + if parse_resultant: + self._auto_latest = parsed_resultant + if parse_taxels: + self._auto_latest_taxels = parsed_taxels + self._auto_latest_ts = time.time() + self._auto_stats.consecutive_errors = 0 # Reset on success + + self._auto_stats.frames_ok += 1 + self._auto_stats.last_error_code = err_code + self._auto_stats.last_eff_len = eff_len + self._auto_stats.last_payload_len = len(valid) + + if parse_success: + self._auto_stats.parse_ok += 1 + else: + self._auto_stats.parse_errors += 1 + self._auto_stats.consecutive_errors += 1 + + # ===== Step 10: Check if we should trigger reconfiguration ===== + if self._auto_stats.consecutive_errors >= ERROR_THRESHOLD: + logger.warning( + f"Consecutive errors ({self._auto_stats.consecutive_errors}) exceeded threshold. " + "Attempting reconfiguration..." + ) + try: + if self._reconfigure(force=False): + logger.info("Reconfiguration successful") + + # Check if we still meet minimum sensor requirement + if self._sensor_config.num_active_sensors < min_sensors: + logger.error( + f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " + f"need {min_sensors}. Stopping stream." + ) + self._auto_running.clear() + break + except NoSensorsAvailableError: + logger.error("No sensors available, stopping stream") + self._auto_running.clear() + break + except Exception as e: + logger.error(f"Reconfiguration failed: {e}") + # Reset error counter to avoid infinite reconfiguration attempts + with self._auto_lock: + self._auto_stats.consecutive_errors = 0 + + except IOError as e: + if "Auto stream stopped" in str(e): + # Normal shutdown, exit gracefully + logger.info("Auto stream stopped") + break + # Other IO errors + logger.warning(f"IO error in auto reader: {e}") + with self._auto_lock: + self._auto_stats.resyncs += 1 + self._auto_stats.consecutive_errors += 1 + time.sleep(0.01) # Brief pause before retry + + except Exception as e: + # Unexpected errors + logger.error(f"Unexpected error in auto reader: {e}", exc_info=True) + with self._auto_lock: + self._auto_stats.resyncs += 1 + self._auto_stats.consecutive_errors += 1 + time.sleep(0.01) # Brief pause before retry + + logger.info("Auto reader loop exited") + + def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1): + """Start continuous auto-stream mode for real-time force data. + + In auto-stream mode, the sensor continuously broadcasts force data at ~1kHz + without requiring request-response polling. This provides much lower latency + and higher throughput than repeatedly calling read_resulting_force(). + + The data is read by a background thread and made available via: + - get_auto_latest(): for resultant force data + - get_auto_latest_taxels(): for taxel data + - get_auto_latest_all(): for both + + The system automatically adapts to sensor configuration changes (connects/disconnects). + + Setup sequence: + 1. Stop any existing stream + 2. Get sensor configuration (which sensors are connected) + 3. Configure data type (0x0016: resultant and/or taxels) + 4. Clear serial buffer to remove stale data + 5. Enable auto transmission (0x0017 = 1) + 6. Start background reader thread + + Args: + resultant: Include resultant force data (fx, fy, fz per sensor) + taxels: Include individual taxel force data + min_sensors: Minimum number of sensors required (default: 1) + If fewer sensors available, raises NoSensorsAvailableError + + Raises: + OSError: If not connected to sensor + NoSensorsAvailableError: If fewer than min_sensors are available + ValueError: If neither resultant nor taxels is enabled + """ + 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") + + # Stop any existing stream to ensure clean state + self.stop_auto_stream() + + # Store mode settings for the reader loop + self._auto_mode_resultant = resultant + self._auto_mode_taxels = taxels + + # Get initial sensor configuration + try: + self._sensor_config = self._get_configuration() + except Exception as e: + raise OSError(f"Failed to get sensor configuration: {e}") from e + + # Check minimum sensor requirement + if self._sensor_config.num_active_sensors < min_sensors: + raise NoSensorsAvailableError( + f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " + f"need at least {min_sensors}" + ) + + # Log expected payload size for debugging + expected_size = self._get_expected_payload_size(self._sensor_config) + mode_str = [] + if resultant: + mode_str.append("resultant") + if taxels: + mode_str.append("taxels") + logger.info( + f"Starting auto-stream with {self._sensor_config}, " + f"mode={'+'.join(mode_str)}, expected_payload={expected_size} bytes" + ) + + # Try to disable auto mode first (in case it was left enabled) + try: + self.disable_auto_data_transmission() + except Exception: + pass # If this fails, robust _write_register will handle AA56 frames + + # Configure which data types to include in auto frames + self.set_auto_data_type(resultant=resultant, taxels=taxels) + + # Clear any stale data from serial buffer before starting + self._serial_connection.reset_input_buffer() + + # Enable auto transmission mode (sensor starts broadcasting) + self.enable_auto_data_transmission() + + # Start background reader thread + self._auto_running.set() # Signal thread to run + self._auto_thread = threading.Thread( + target=self._auto_reader_loop, + args=(resultant, taxels, min_sensors), + daemon=True # Thread exits when main program exits + ) + self._auto_thread.start() + + + def stop_auto_stream(self): + """Stop auto-stream mode and clean up background thread. + + Sequence: + 1. Signal thread to stop (_auto_running.clear()) + 2. Wait up to 1.0s for thread to exit gracefully + 3. Disable auto transmission on sensor (0x0017 = 0) + 4. Reset cached data + + This method is safe to call multiple times and handles cases where + the sensor is disconnected or the thread is already stopped. + """ + # Signal background thread to stop + self._auto_running.clear() + + # Wait for thread to exit (up to 1 second) + if self._auto_thread is not None: + self._auto_thread.join(timeout=1.0) + self._auto_thread = None + + # Disable auto mode on sensor (if still connected) + if self.is_connected: + try: + self.disable_auto_data_transmission() + except Exception: + pass # Ignore errors (e.g., if sensor disconnected) + + # Reset cached data + with self._auto_lock: + self._auto_latest = None + self._auto_latest_taxels = None + self._auto_latest_ts = None + + + +if __name__ == "__main__": + import sys + + sensor_client = SensorClient(port="/dev/ttyACM0", baudrate=921600) + + sensor_client.connect() + print(sensor_client._sensor_config) + exit() + try: + print("version:", sensor_client.read_hardware_version()) + print("connected sensors:", sensor_client.read_connected_sensors()) + print("num taxels:", sensor_client.read_num_taxels()) + print("config:", sensor_client.get_sensor_configuration()) + + # Parse command line for mode selection + mode = sys.argv[1] if len(sys.argv) > 1 else "resultant" + + if mode == "resultant": + print("\n=== Resultant Force Only Mode ===") + sensor_client.start_auto_stream(resultant=True, taxels=False) + for _ in range(100): + forces, ts = sensor_client.get_auto_latest() + if forces is not None: + print(f"[{ts:.3f}] forces: {forces}") + time.sleep(0.05) + + elif mode == "taxels": + print("\n=== Taxels Only Mode ===") + sensor_client.start_auto_stream(resultant=False, taxels=True) + for _ in range(100): + taxels, ts = sensor_client.get_auto_latest_taxels() + if taxels is not None: + # Print summary: count and first taxel [fx, fy, fz] per finger + summary = {f: (len(vals), vals[0] if vals else []) for f, vals in taxels.items()} + print(f"[{ts:.3f}] taxels (count, first): {summary}") + time.sleep(0.05) + + elif mode == "combined": + print("\n=== Combined Mode (Resultant + Taxels) ===") + sensor_client.start_auto_stream(resultant=True, taxels=True) + for _ in range(100): + forces, taxels, ts = sensor_client.get_auto_latest_all() + if forces is not None: + taxel_summary = {f: len(vals) for f, vals in taxels.items()} if taxels else {} + print(f"[{ts:.3f}] forces: {forces}, taxel_counts: {taxel_summary}") + time.sleep(0.05) + + else: + print(f"Unknown mode: {mode}. Use 'resultant', 'taxels', or 'combined'") + + print("\nstats:", sensor_client.get_auto_stats()) + + finally: + try: + sensor_client.stop_auto_stream() + except Exception: + pass + sensor_client.disconnect() \ No newline at end of file diff --git a/orca_core/hardware/sensing/taxel_coordinates.py b/orca_core/hardware/sensing/taxel_coordinates.py new file mode 100644 index 00000000..c0785910 --- /dev/null +++ b/orca_core/hardware/sensing/taxel_coordinates.py @@ -0,0 +1,85 @@ +"""Config-driven taxel coordinates for tactile sensors. + +Coordinates are loaded from sensor model config files in the models/ directory. +The finger-to-model mapping is configured in models/sensor_models.yaml. +""" + +import os +from typing import TypedDict, Optional +import yaml + + +class TaxelCoord(TypedDict): + x: float + y: float + z: float + + +MODELS_DIR = os.path.join(os.path.dirname(__file__), "models") +SENSOR_MODELS_CONFIG = os.path.join(MODELS_DIR, "sensor_models.yaml") + +_model_cache: dict[str, list[TaxelCoord]] = {} +_finger_mapping_cache: Optional[dict[str, str]] = None + + +def _load_finger_mapping() -> dict[str, str]: + """Load the finger-to-model mapping from sensor_models.yaml.""" + global _finger_mapping_cache + if _finger_mapping_cache is not None: + return _finger_mapping_cache + + with open(SENSOR_MODELS_CONFIG, "r") as f: + config = yaml.safe_load(f) + + _finger_mapping_cache = config["finger_models"] + return _finger_mapping_cache + + +def _load_model_coordinates(model_name: str) -> list[TaxelCoord]: + """Load coordinates for a sensor model from its config.yaml.""" + if model_name in _model_cache: + return _model_cache[model_name] + + config_path = os.path.join(MODELS_DIR, model_name, "config.yaml") + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + coords = [TaxelCoord(x=c["x"], y=c["y"], z=c["z"]) for c in config["coordinates"]] + _model_cache[model_name] = coords + return coords + + +def get_coordinates(finger: str) -> list[TaxelCoord]: + """Get taxel coordinates for a finger. + + Args: + finger: Finger name ('thumb', 'index', 'middle', 'ring', 'pinky') + + Returns: + List of coordinate dicts with 'x', 'y', 'z' keys (in mm) + """ + mapping = _load_finger_mapping() + model_name = mapping.get(finger) + if model_name is None: + return [] + return _load_model_coordinates(model_name) + + +def get_all_coordinates() -> dict[str, list[TaxelCoord]]: + """Get taxel coordinates for all fingers. + + Returns: + Dict mapping finger name to list of coordinate dicts + """ + mapping = _load_finger_mapping() + return {finger: _load_model_coordinates(model) for finger, model in mapping.items()} + + +def get_taxel_counts() -> dict[str, int]: + """Get the taxel count for each finger based on the current model mapping. + + Returns: + Dict mapping finger name to number of taxels + """ + mapping = _load_finger_mapping() + return {finger: len(_load_model_coordinates(model)) for finger, model in mapping.items()} diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 2b4717d4..83afe461 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -18,7 +18,7 @@ from .base_hand import BaseHand from .calibration import CalibrationResult -from .hand_config import OrcaHandConfig +from .hand_config import OrcaHandConfig, OrcaHandTouchConfig from .hardware.motor_client import MotorClient from .utils.utils import auto_detect_port, get_and_choose_port, update_yaml @@ -1141,6 +1141,95 @@ def stop_task(self): print("No running task to stop.") +class OrcaHandTouch(OrcaHand): + """ORCA hand with integrated tactile sensing. + + Extends :class:`OrcaHand` to additionally manage a tactile sensor array. + Connection, disconnection, and lifecycle are unified: calling + :meth:`connect` opens both the motor bus and the sensor serial link, + and :meth:`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._sensor_client = None + + def connect(self) -> tuple[bool, str]: + success, msg = super().connect() + if not success: + return success, msg + + from .hardware.sensing.sensor_client import SensorClient + + self._sensor_client = SensorClient( + port=self.config.sensor_port, + baudrate=self.config.sensor_baudrate, + finger_to_sensor_id=self.config.finger_to_sensor_id, + ) + try: + self._sensor_client.connect() + except Exception as e: + self._sensor_client = None + return False, f"{msg} | Sensor connection failed: {e}" + + return True, f"{msg} | Sensor connected" + + def disconnect(self) -> None: + if self._sensor_client is not None and self._sensor_client.is_connected: + try: + self._sensor_client.stop_auto_stream() + except Exception: + pass + self._sensor_client.disconnect() + self._sensor_client = None + super().disconnect() + + def get_tactile_forces(self) -> dict[str, list[float]]: + """Return latest resultant force per finger ``{finger: [fx, fy, fz]}``.""" + forces, _ = self._sensor_client.get_auto_latest() + return forces + + def get_tactile_taxels(self) -> dict[str, list[list[float]]]: + """Return per-taxel forces ``{finger: [[fx, fy, fz], ...]}``.""" + taxels, _ = self._sensor_client.get_auto_latest_taxels() + return taxels + + def start_tactile_stream( + self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1 + ) -> None: + self._sensor_client.start_auto_stream( + resultant=resultant, taxels=taxels, min_sensors=min_sensors, + ) + + def stop_tactile_stream(self) -> None: + self._sensor_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._sensor_client.capture_taxel_offsets(num_samples=num_samples) + + def clear_tactile_zero(self) -> None: + self._sensor_client.clear_taxel_offsets() + + def get_sensor_configuration(self): + return self._sensor_client.get_sensor_configuration() + + class MockOrcaHand(OrcaHand): """Drop-in :class:`OrcaHand` backed by an in-memory mock motor client, for testing and prototyping. 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..25a98e11 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -16,6 +16,13 @@ dependencies = [ "numpy (>=2.2.6,<3.0.0)", ] +[project.optional-dependencies] +sensing-ui = [ + "flask>=3.0.0,<4.0.0", + "flask-socketio>=5.6.0,<6.0.0", + "pyserial>=3.5,<4.0.0", +] + [dependency-groups] dev = [ "matplotlib>=3.10.1,<4.0.0", diff --git a/scripts/tactile_sensing_ui/README.md b/scripts/tactile_sensing_ui/README.md new file mode 100644 index 00000000..ead2f4fe --- /dev/null +++ b/scripts/tactile_sensing_ui/README.md @@ -0,0 +1,26 @@ +# ORCA Tactile Sensing UI + +Web-based interface for real-time tactile sensor visualization. + +## Usage + +```bash +python scripts/tactile_sensing_ui/tactile_ui.py +python scripts/tactile_sensing_ui/tactile_ui.py --config orca_core/models/v2/orcahand-touch/config.yaml +``` + +Then open your browser to `http://localhost:5001` + +## Features + +- **Connection Management**: Connect/disconnect to sensor devices +- **Sensor Status**: View which sensors are connected +- **Taxel Counts**: Display number of taxels for each sensor +- **Force Visualization**: Real-time force vectors displayed as arrows and numerical values +- **Taxel Visualization**: 2D taxel view with magnitude, direction, and arrow display modes +- **Zeroing**: Capture sensor baseline offsets +- **Auto Update**: Continuous monitoring of sensor data via WebSocket + +## Dependencies + +Install with: `pip install -e ".[sensing-ui]"` diff --git a/scripts/tactile_sensing_ui/static/script.js b/scripts/tactile_sensing_ui/static/script.js new file mode 100644 index 00000000..7be78654 --- /dev/null +++ b/scripts/tactile_sensing_ui/static/script.js @@ -0,0 +1,879 @@ +const MAX_FORCE_SCALE = 10; +const MIN_CIRCLE_RADIUS = 2; +const MAX_CIRCLE_RADIUS = 40; +const VISUALIZATION_RADIUS = 70; +const MAX_TAXEL_FORCE = 5; + +const socket = io(); + +// State +let currentMode = 'taxels'; +let taxelDisplayMode = 'direction'; // 'magnitude', 'direction', or 'arrows' +let arrowColorScheme = 'heat'; // 'heat', 'intensity', or 'orca' +let forceThreshold = 0.5; // default threshold in N +let arrowLengthMult = 1.0; +let arrowThicknessMult = 1.0; +let taxelCounts = { thumb: 127, index: 52, middle: 31, ring: 31, pinky: 31 }; +let taxelGridsInitialized = false; +let activeSensors = {}; +let taxelCoordinates = null; // Will be fetched from server + +socket.on('connect', () => { + console.log('WebSocket connected'); +}); + +socket.on('disconnect', () => { + console.log('WebSocket disconnected'); +}); + +socket.on('force_update', (forces) => { + updateForces(forces); + updateActiveSensorsFromForces(forces); +}); + +socket.on('taxel_update', (taxels) => { + updateTaxels(taxels); + updateActiveSensorsFromTaxels(taxels); +}); + +socket.on('combined_update', (data) => { + if (data.forces) { + updateForces(data.forces); + updateActiveSensorsFromForces(data.forces); + } + if (data.taxels) { + updateTaxels(data.taxels); + updateActiveSensorsFromTaxels(data.taxels); + } +}); + +socket.on('mode_changed', (data) => { + currentMode = data.mode; + updatePanelVisibility(); + document.getElementById('current-mode').textContent = getModeLabel(data.mode); + updateAutoDataTypeDisplay(data.mode); +}); + +socket.on('connection_status', (data) => { + if (data.connected) { + document.getElementById('connection-status').textContent = 'Connected'; + document.getElementById('connection-status').className = 'status-indicator connected'; + document.getElementById('connect-btn').disabled = true; + document.getElementById('disconnect-btn').disabled = false; + document.getElementById('refresh-btn').disabled = false; + if (data.mode) { + currentMode = data.mode; + document.getElementById('mode-select').value = data.mode; + } + updateUI(); + } else { + document.getElementById('connection-status').textContent = 'Disconnected'; + document.getElementById('connection-status').className = 'status-indicator disconnected'; + if (data.error) { + showError(data.error); + } + } +}); + +socket.on('error', (data) => { + showError(data.message); +}); + +socket.on('config_update', (data) => { + console.log('Sensor configuration changed:', data); + updateSensorConfig(data); +}); + +let currentView = '2d'; + +document.getElementById('connect-btn').addEventListener('click', connect); +document.getElementById('disconnect-btn').addEventListener('click', disconnect); +document.getElementById('refresh-btn').addEventListener('click', refresh); +document.getElementById('zero-btn').addEventListener('click', zeroSensors); +document.getElementById('reset-zero-btn').addEventListener('click', resetZero); +document.getElementById('scan-btn').addEventListener('click', scanPorts); +document.getElementById('mode-select').addEventListener('change', changeMode); +document.getElementById('magnitude-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('magnitude')); +document.getElementById('direction-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('direction')); +document.getElementById('arrows-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('arrows')); +document.getElementById('color-scheme-select').addEventListener('change', (e) => { + arrowColorScheme = e.target.value; +}); +document.getElementById('arrow-length-slider').addEventListener('input', (e) => { + arrowLengthMult = parseFloat(e.target.value); + document.getElementById('arrow-length-value').textContent = arrowLengthMult.toFixed(1) + 'x'; + window.dispatchEvent(new CustomEvent('arrow-size-changed', { detail: { length: arrowLengthMult, thickness: arrowThicknessMult } })); +}); +document.getElementById('arrow-thickness-slider').addEventListener('input', (e) => { + arrowThicknessMult = parseFloat(e.target.value); + document.getElementById('arrow-thickness-value').textContent = arrowThicknessMult.toFixed(1) + 'x'; + window.dispatchEvent(new CustomEvent('arrow-size-changed', { detail: { length: arrowLengthMult, thickness: arrowThicknessMult } })); +}); +document.getElementById('threshold-toggle').addEventListener('change', (e) => { + const input = document.getElementById('threshold-input'); + input.disabled = !e.target.checked; + forceThreshold = e.target.checked ? parseFloat(input.value) || 0 : 0; + window.dispatchEvent(new CustomEvent('threshold-changed', { detail: { threshold: forceThreshold } })); +}); +document.getElementById('threshold-input').addEventListener('input', (e) => { + const toggle = document.getElementById('threshold-toggle'); + if (toggle.checked) { + forceThreshold = parseFloat(e.target.value) || 0; + window.dispatchEvent(new CustomEvent('threshold-changed', { detail: { threshold: forceThreshold } })); + } +}); + +// Auto-scan on page load +scanPorts(); + +function getModeLabel(mode) { + switch (mode) { + case 'resultant': return 'Resultant Force'; + case 'taxels': return 'Taxels Only'; + case 'combined': return 'Combined'; + default: return mode; + } +} + +async function scanPorts() { + const select = document.getElementById('port-select'); + const scanBtn = document.getElementById('scan-btn'); + scanBtn.disabled = true; + scanBtn.textContent = '...'; + try { + const response = await fetch('/api/ports'); + const ports = await response.json(); + const previousValue = select.value; + select.innerHTML = ''; + if (ports.length === 0) { + const opt = document.createElement('option'); + opt.value = '/dev/ttyACM0'; + opt.textContent = 'No ports found — /dev/ttyACM0'; + select.appendChild(opt); + } else { + ports.forEach(p => { + const opt = document.createElement('option'); + opt.value = p.device; + const label = p.is_sensor_adapter ? `${p.device} (Sensor Adapter)` : `${p.device} — ${p.description}`; + opt.textContent = label; + if (p.is_sensor_adapter) opt.style.fontWeight = '600'; + select.appendChild(opt); + }); + // Re-select previous value if still present, otherwise keep first (best match) + if ([...select.options].some(o => o.value === previousValue)) { + select.value = previousValue; + } + } + } catch (error) { + showError('Port scan failed: ' + error.message); + } finally { + scanBtn.disabled = false; + scanBtn.textContent = 'Scan'; + } +} + +async function fetchTaxelCoordinates() { + try { + const response = await fetch('/api/taxel_coordinates'); + taxelCoordinates = await response.json(); + return taxelCoordinates; + } catch (error) { + console.error('Failed to fetch taxel coordinates:', error); + return null; + } +} + +async function connect() { + const port = document.getElementById('port-select').value; + const mode = document.getElementById('mode-select').value; + try { + // Fetch coordinates before connecting + if (!taxelCoordinates) { + await fetchTaxelCoordinates(); + } + + const response = await fetch('/api/connect', { + method: 'POST', + headers: {'Content-Type': 'application/json'}, + body: JSON.stringify({port: port, mode: mode}) + }); + const data = await response.json(); + if (data.success) { + showError(null); + currentMode = data.mode; + if (data.config && data.config.num_taxels) { + taxelCounts = data.config.num_taxels; + } + document.getElementById('zero-btn').disabled = false; + initializeTaxelGrids(); + updatePanelVisibility(); + updateAutoDataTypeDisplay(data.mode); + updateUI(); + } else { + showError(data.message); + } + } catch (error) { + showError('Connection failed: ' + error.message); + } +} + +async function disconnect() { + try { + const response = await fetch('/api/disconnect', {method: 'POST'}); + const data = await response.json(); + if (data.success) { + showError(null); + document.getElementById('connection-status').textContent = 'Disconnected'; + document.getElementById('connection-status').className = 'status-indicator disconnected'; + document.getElementById('connect-btn').disabled = false; + document.getElementById('disconnect-btn').disabled = true; + document.getElementById('refresh-btn').disabled = true; + document.getElementById('zero-btn').disabled = true; + document.getElementById('reset-zero-btn').disabled = true; + document.getElementById('reset-zero-btn').style.display = 'none'; + document.getElementById('stream-status').style.display = 'none'; + } + } catch (error) { + showError('Disconnect failed: ' + error.message); + } +} + +async function zeroSensors() { + const btn = document.getElementById('zero-btn'); + btn.disabled = true; + btn.textContent = 'Zeroing...'; + try { + const response = await fetch('/api/zero', {method: 'POST'}); + const data = await response.json(); + if (data.success) { + showError(null); + document.getElementById('reset-zero-btn').style.display = ''; + document.getElementById('reset-zero-btn').disabled = false; + } else { + showError(data.message); + } + } catch (error) { + showError('Zero failed: ' + error.message); + } finally { + btn.disabled = false; + btn.textContent = 'Zero'; + } +} + +async function resetZero() { + try { + const response = await fetch('/api/clear_zero', {method: 'POST'}); + const data = await response.json(); + if (data.success) { + showError(null); + document.getElementById('reset-zero-btn').style.display = 'none'; + document.getElementById('reset-zero-btn').disabled = true; + } else { + showError(data.message); + } + } catch (error) { + showError('Reset zero failed: ' + error.message); + } +} + +async function changeMode() { + const mode = document.getElementById('mode-select').value; + try { + const response = await fetch('/api/mode', { + method: 'POST', + headers: {'Content-Type': 'application/json'}, + body: JSON.stringify({mode: mode}) + }); + const data = await response.json(); + if (data.success) { + currentMode = data.mode; + updatePanelVisibility(); + document.getElementById('current-mode').textContent = getModeLabel(data.mode); + updateAutoDataTypeDisplay(data.mode); + } else { + showError(data.message); + document.getElementById('mode-select').value = currentMode; + } + } catch (error) { + showError('Mode change failed: ' + error.message); + document.getElementById('mode-select').value = currentMode; + } +} + +function setTaxelDisplayMode(mode) { + taxelDisplayMode = mode; + + const magLegend = document.querySelector('.magnitude-legend'); + const dirLegend = document.querySelector('.direction-legend'); + const arrowsLegend = document.querySelector('.arrows-legend'); + + magLegend.style.display = mode === 'magnitude' ? 'inline-block' : 'none'; + dirLegend.style.display = mode === 'direction' ? 'inline-block' : 'none'; + arrowsLegend.style.display = mode === 'arrows' ? 'inline-block' : 'none'; + + // Clear arrows when switching away from arrows mode + if (mode !== 'arrows') { + clearAllArrows(); + } +} + +function clearAllArrows() { + document.querySelectorAll('.taxel-arrow').forEach(el => el.remove()); +} + +function updatePanelVisibility() { + const forcesPanel = document.getElementById('forces-panel'); + const taxelsPanel = document.getElementById('taxels-panel'); + + switch (currentMode) { + case 'resultant': + forcesPanel.style.display = 'block'; + taxelsPanel.style.display = 'none'; + break; + case 'taxels': + forcesPanel.style.display = 'none'; + taxelsPanel.style.display = 'block'; + break; + case 'combined': + forcesPanel.style.display = 'block'; + taxelsPanel.style.display = 'block'; + break; + } +} + +function initializeTaxelGrids() { + const container = document.getElementById('taxels-container'); + container.innerHTML = ''; + + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + + fingers.forEach(finger => { + const coords = taxelCoordinates ? taxelCoordinates[finger] : null; + const numTaxels = coords ? coords.length : (taxelCounts[finger] || 31); + + const fingerDiv = document.createElement('div'); + fingerDiv.className = 'taxel-finger'; + fingerDiv.dataset.finger = finger; + + const label = document.createElement('div'); + label.className = 'taxel-finger-label'; + label.textContent = finger.charAt(0).toUpperCase() + finger.slice(1); + fingerDiv.appendChild(label); + + if (coords && coords.length > 0) { + // Use coordinate-based SVG rendering + const svg = createCoordinateSVG(finger, coords); + fingerDiv.appendChild(svg); + } else { + // Fallback to simple grid if no coordinates + const grid = createFallbackGrid(finger, numTaxels); + fingerDiv.appendChild(grid); + } + + container.appendChild(fingerDiv); + }); + + taxelGridsInitialized = true; +} + +function createCoordinateSVG(finger, coords) { + // Calculate bounds + let minX = Infinity, maxX = -Infinity; + let minY = Infinity, maxY = -Infinity; + + coords.forEach(c => { + minX = Math.min(minX, c.x); + maxX = Math.max(maxX, c.x); + minY = Math.min(minY, c.y); + maxY = Math.max(maxY, c.y); + }); + + const dataWidth = maxX - minX; + const dataHeight = maxY - minY; + + // SVG dimensions - scale based on finger size + const padding = 8; + const taxelRadius = finger === 'thumb' ? 5 : 5; + const scale = finger === 'thumb' ? 7.5 : 7; + + const svgWidth = dataWidth * scale + padding * 2; + const svgHeight = dataHeight * scale + padding * 2; + + const svg = document.createElementNS('http://www.w3.org/2000/svg', 'svg'); + svg.setAttribute('class', 'taxel-svg'); + svg.setAttribute('viewBox', `0 0 ${svgWidth} ${svgHeight}`); + svg.setAttribute('width', svgWidth); + svg.setAttribute('height', svgHeight); + svg.dataset.finger = finger; + + // Add taxel circles + coords.forEach((coord, index) => { + const circle = document.createElementNS('http://www.w3.org/2000/svg', 'circle'); + + // Transform coordinates to SVG space + // X: left-to-right maps to SVG x + // Y: sensor Y (proximal-to-distal) maps to SVG y (top-to-bottom, inverted) + const svgX = (coord.x - minX) * scale + padding; + const svgY = svgHeight - ((coord.y - minY) * scale + padding); // Invert Y + + circle.setAttribute('cx', svgX); + circle.setAttribute('cy', svgY); + circle.setAttribute('r', taxelRadius); + circle.setAttribute('fill', '#1a1a1a'); + circle.setAttribute('stroke', '#2a2a2a'); + circle.setAttribute('stroke-width', '0.5'); + circle.setAttribute('class', 'taxel-circle'); + circle.setAttribute('id', `taxel-${finger}-${index}`); + circle.dataset.taxelIndex = index; + + svg.appendChild(circle); + }); + + return svg; +} + +function createFallbackGrid(finger, numTaxels) { + // Simple fallback grid when coordinates are not available + const grid = document.createElement('div'); + grid.className = 'taxel-grid'; + grid.dataset.finger = finger; + + const cols = finger === 'thumb' ? 11 : 6; + const rows = Math.ceil(numTaxels / cols); + + let taxelIndex = 0; + for (let row = 0; row < rows; row++) { + const rowDiv = document.createElement('div'); + rowDiv.className = 'taxel-row'; + + for (let col = 0; col < cols && taxelIndex < numTaxels; col++) { + const cell = document.createElement('div'); + cell.className = 'taxel-cell'; + cell.dataset.taxelIndex = taxelIndex; + cell.id = `taxel-${finger}-${taxelIndex}`; + rowDiv.appendChild(cell); + taxelIndex++; + } + + grid.appendChild(rowDiv); + } + + return grid; +} + +function updateTaxels(taxels) { + if (!taxelGridsInitialized) return; + + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + + fingers.forEach(finger => { + const fingerTaxels = taxels[finger]; + if (!fingerTaxels) return; + + fingerTaxels.forEach((taxelData, index) => { + const element = document.getElementById(`taxel-${finger}-${index}`); + if (!element) return; + + const [fx, fy, fz] = taxelData; + const magnitude = Math.sqrt(fx * fx + fy * fy + fz * fz); + + // Check if it's an SVG circle or a div cell + const isSVG = element.tagName.toLowerCase() === 'circle'; + + // If below threshold, reset to default and skip + if (forceThreshold > 0 && magnitude < forceThreshold) { + if (isSVG) { + element.setAttribute('fill', '#1a1a1a'); + } else { + element.style.backgroundColor = '#1a1a1a'; + } + return; + } + + if (taxelDisplayMode === 'arrows' && isSVG) { + // Arrows mode - show 3D direction with intensity coloring + updateTaxelArrow(element, fx, fy, fz, magnitude); + } else if (taxelDisplayMode === 'direction') { + // Direction-based coloring + const absX = Math.abs(fx); + const absY = Math.abs(fy); + + let color = '#1a1a1a'; + if (magnitude >= 0.1) { + const alpha = Math.min(magnitude / MAX_TAXEL_FORCE, 1); + const opacity = 0.3 + alpha * 0.7; + + if (absX > absY) { + if (fx > 0) { + color = `rgba(239, 68, 68, ${opacity})`; + } else { + color = `rgba(6, 182, 212, ${opacity})`; + } + } else { + if (fy > 0) { + color = `rgba(16, 185, 129, ${opacity})`; + } else { + color = `rgba(245, 158, 11, ${opacity})`; + } + } + } + + if (isSVG) { + element.setAttribute('fill', color); + } else { + element.style.backgroundColor = color; + } + } else { + // Magnitude-based coloring (grayscale) + const normalized = Math.min(magnitude / MAX_TAXEL_FORCE, 1); + const gray = Math.round(60 + normalized * 180); + const color = `rgb(${gray}, ${gray}, ${gray})`; + + if (isSVG) { + element.setAttribute('fill', color); + } else { + element.style.backgroundColor = color; + } + } + }); + }); +} + +function getArrowColor2D(normalized) { + switch (arrowColorScheme) { + case 'intensity': { + const l = Math.round(15 + normalized * 85); + return `hsl(0, 0%, ${l}%)`; + } + case 'orca': { + // ORCA palette gradient: #474f5e → #7f8ea2 → #bfc7d1 → #e5e7eb + const stops = [ + [71, 79, 94], // #474f5e + [127, 142, 162], // #7f8ea2 + [191, 199, 209], // #bfc7d1 + [229, 231, 235], // #e5e7eb + ]; + const scaled = normalized * (stops.length - 1); + const idx = Math.min(Math.floor(scaled), stops.length - 2); + const frac = scaled - idx; + const r = Math.round(stops[idx][0] + (stops[idx + 1][0] - stops[idx][0]) * frac); + const g = Math.round(stops[idx][1] + (stops[idx + 1][1] - stops[idx][1]) * frac); + const b = Math.round(stops[idx][2] + (stops[idx + 1][2] - stops[idx][2]) * frac); + return `rgb(${r}, ${g}, ${b})`; + } + default: { // 'heat' + const hue = (1 - normalized) * 240; + const saturation = 70 + normalized * 30; + const lightness = 55 - normalized * 10; + return `hsl(${hue}, ${saturation}%, ${lightness}%)`; + } + } +} + +function updateTaxelArrow(circleElement, fx, fy, fz, magnitude) { + const svg = circleElement.closest('svg'); + if (!svg) return; + + const cx = parseFloat(circleElement.getAttribute('cx')); + const cy = parseFloat(circleElement.getAttribute('cy')); + const taxelId = circleElement.id; + const arrowId = `arrow-${taxelId}`; + + // Remove existing arrow + const existingArrow = document.getElementById(arrowId); + if (existingArrow) existingArrow.remove(); + + // Reset circle to light gray background + circleElement.setAttribute('fill', '#0a0a0a'); + + // Don't draw arrow if force is too small or below threshold + if (magnitude < 0.1) return; + if (forceThreshold > 0 && magnitude < forceThreshold) return; + + // Calculate arrow properties + const normalized = Math.min(magnitude / MAX_TAXEL_FORCE, 1); + + // XY magnitude for arrow direction in plane + const xyMag = Math.sqrt(fx * fx + fy * fy); + + // Arrow length based on XY magnitude, with Z affecting it + const baseLength = (4 + normalized * 10) * arrowLengthMult; + const zFactor = 1 + Math.abs(fz) / MAX_TAXEL_FORCE * 0.5; + const arrowLength = baseLength * (xyMag > 0.1 ? 1 : 0.3) * zFactor; + + // Arrow direction (in SVG coordinates, Y is inverted) + let angle = 0; + if (xyMag > 0.1) { + angle = Math.atan2(-fy, fx); // Negative fy because SVG Y is down + } + + // End point of arrow + const endX = cx + Math.cos(angle) * arrowLength; + const endY = cy + Math.sin(angle) * arrowLength; + + // Color based on selected scheme + const color = getArrowColor2D(normalized); + + // Create arrow group + const arrowGroup = document.createElementNS('http://www.w3.org/2000/svg', 'g'); + arrowGroup.setAttribute('id', arrowId); + arrowGroup.setAttribute('class', 'taxel-arrow'); + + // Arrow line + const line = document.createElementNS('http://www.w3.org/2000/svg', 'line'); + line.setAttribute('x1', cx); + line.setAttribute('y1', cy); + line.setAttribute('x2', endX); + line.setAttribute('y2', endY); + line.setAttribute('stroke', color); + line.setAttribute('stroke-width', (2 + normalized * 2.5) * arrowThicknessMult); + line.setAttribute('stroke-linecap', 'round'); + arrowGroup.appendChild(line); + + // Arrowhead (triangle) + if (arrowLength > 4) { + const headLength = (3 + normalized * 3) * arrowThicknessMult; + const headAngle = 0.6; // radians, about 35 degrees + + const head1X = endX - Math.cos(angle - headAngle) * headLength; + const head1Y = endY - Math.sin(angle - headAngle) * headLength; + const head2X = endX - Math.cos(angle + headAngle) * headLength; + const head2Y = endY - Math.sin(angle + headAngle) * headLength; + + const arrowhead = document.createElementNS('http://www.w3.org/2000/svg', 'polygon'); + arrowhead.setAttribute('points', `${endX},${endY} ${head1X},${head1Y} ${head2X},${head2Y}`); + arrowhead.setAttribute('fill', color); + arrowGroup.appendChild(arrowhead); + } + + svg.appendChild(arrowGroup); +} + +async function refresh() { + try { + const response = await fetch('/api/refresh'); + const data = await response.json(); + if (data.connected) { + updateStatus(data); + if (data.forces) updateForces(data.forces); + if (data.mode) { + currentMode = data.mode; + document.getElementById('mode-select').value = data.mode; + updatePanelVisibility(); + } + } else { + showError(data.error || 'Not connected'); + } + } catch (error) { + showError('Refresh failed: ' + error.message); + } +} + +async function updateUI() { + try { + // Ensure we have coordinates + if (!taxelCoordinates) { + await fetchTaxelCoordinates(); + } + + const response = await fetch('/api/status'); + const data = await response.json(); + if (data.connected) { + document.getElementById('connection-status').textContent = 'Connected'; + document.getElementById('connection-status').className = 'status-indicator connected'; + document.getElementById('connect-btn').disabled = true; + document.getElementById('disconnect-btn').disabled = false; + document.getElementById('refresh-btn').disabled = false; + document.getElementById('stream-status').style.display = 'block'; + updateStatus(data); + + if (data.taxels) { + taxelCounts = data.taxels; + if (!taxelGridsInitialized) { + initializeTaxelGrids(); + } + } + + if (data.mode) { + currentMode = data.mode; + document.getElementById('mode-select').value = data.mode; + document.getElementById('current-mode').textContent = getModeLabel(data.mode); + updatePanelVisibility(); + } + + const forcesResponse = await fetch('/api/forces'); + const forces = await forcesResponse.json(); + updateForces(forces); + } else { + document.getElementById('connection-status').textContent = 'Disconnected'; + document.getElementById('connection-status').className = 'status-indicator disconnected'; + } + } catch (error) { + showError('Update failed: ' + error.message); + } +} + +function updateAutoDataTypeDisplay(mode) { + const resultant = mode === 'resultant' || mode === 'combined'; + const taxels = mode === 'taxels' || mode === 'combined'; + document.getElementById('auto-data-type').innerHTML = ` + Resultant Force: ${resultant ? '✓' : '✗'}
+ Individual Taxels: ${taxels ? '✓' : '✗'} + `; +} + +function updateStatus(data) { + document.getElementById('hardware-version').textContent = data.hardware_version || '-'; + + if (data.mode) { + document.getElementById('current-mode').textContent = getModeLabel(data.mode); + updateAutoDataTypeDisplay(data.mode); + } + + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + fingers.forEach(finger => { + const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); + const status = card.querySelector('.sensor-status'); + const taxels = card.querySelector('.sensor-taxels span'); + + if (data.sensors && data.sensors[finger]) { + status.className = 'sensor-status connected'; + status.textContent = '●'; + if (data.taxels) { + taxels.textContent = data.taxels[finger] || 0; + } + } else { + status.className = 'sensor-status disconnected'; + status.textContent = '●'; + taxels.textContent = '-'; + } + }); +} + +function updateForces(forces) { + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + + fingers.forEach(finger => { + if (!forces[finger]) return; + + const [fx, fy, fz] = forces[finger]; + const magnitude = Math.sqrt(fx*fx + fy*fy + fz*fz); + + document.getElementById(`fx-${finger}`).textContent = fx.toFixed(1); + document.getElementById(`fy-${finger}`).textContent = fy.toFixed(1); + document.getElementById(`fz-${finger}`).textContent = fz.toFixed(1); + document.getElementById(`mag-${finger}`).textContent = magnitude.toFixed(1); + + const circle = document.getElementById(`force-circle-${finger}`); + if (circle) { + const centerX = 100; + const centerY = 100; + + const normalizedMagnitude = Math.min(magnitude / MAX_FORCE_SCALE, 1); + const radius = MIN_CIRCLE_RADIUS + (normalizedMagnitude * (MAX_CIRCLE_RADIUS - MIN_CIRCLE_RADIUS)); + + const angle = Math.atan2(fy, fx); + const distance = Math.min(normalizedMagnitude * VISUALIZATION_RADIUS, VISUALIZATION_RADIUS); + const circleX = centerX + distance * Math.cos(angle); + const circleY = centerY - distance * Math.sin(angle); + + circle.setAttribute('cx', circleX); + circle.setAttribute('cy', circleY); + circle.setAttribute('r', radius); + + const opacity = Math.min(0.3 + normalizedMagnitude * 0.7, 1); + const color = magnitude > 1 ? '#ef4444' : '#3b82f6'; + circle.setAttribute('fill', color); + circle.setAttribute('opacity', opacity); + } + }); +} + + +function showError(message) { + const panel = document.getElementById('error-panel'); + const msg = document.getElementById('error-message'); + if (message) { + msg.textContent = message; + panel.style.display = 'block'; + } else { + panel.style.display = 'none'; + } +} + +function updateActiveSensorsFromForces(forces) { + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + fingers.forEach(finger => { + if (forces[finger]) { + activeSensors[finger] = true; + updateSensorStatusDisplay(finger, true); + } + }); +} + +function updateActiveSensorsFromTaxels(taxels) { + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + fingers.forEach(finger => { + if (taxels[finger] && taxels[finger].length > 0) { + activeSensors[finger] = true; + updateSensorStatusDisplay(finger, true, taxels[finger].length); + } + }); +} + +function updateSensorStatusDisplay(finger, connected, taxelCount) { + const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); + if (!card) return; + + const status = card.querySelector('.sensor-status'); + if (connected) { + status.className = 'sensor-status connected'; + status.textContent = '●'; + } else { + status.className = 'sensor-status disconnected'; + status.textContent = '●'; + } + + if (taxelCount !== undefined) { + const taxelsSpan = card.querySelector('.sensor-taxels span'); + if (taxelsSpan) { + taxelsSpan.textContent = taxelCount; + } + } +} + +function updateSensorConfig(data) { + const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; + + fingers.forEach(finger => { + const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); + if (!card) return; + + const status = card.querySelector('.sensor-status'); + const taxelsSpan = card.querySelector('.sensor-taxels span'); + + const isConnected = data.sensors && data.sensors[finger]; + + if (isConnected) { + status.className = 'sensor-status connected'; + status.textContent = '●'; + if (taxelsSpan && data.taxels) { + taxelsSpan.textContent = data.taxels[finger] || 0; + } + } else { + status.className = 'sensor-status disconnected'; + status.textContent = '●'; + if (taxelsSpan) { + taxelsSpan.textContent = '-'; + } + } + + // Update activeSensors tracking + activeSensors[finger] = isConnected; + }); + + // Update taxel counts for grid reinitialization if needed + if (data.taxels) { + taxelCounts = data.taxels; + } +} diff --git a/scripts/tactile_sensing_ui/static/style.css b/scripts/tactile_sensing_ui/static/style.css new file mode 100644 index 00000000..916a893a --- /dev/null +++ b/scripts/tactile_sensing_ui/static/style.css @@ -0,0 +1,628 @@ +* { + margin: 0; + padding: 0; + box-sizing: border-box; +} + +body { + font-family: 'Space Mono', monospace; + background: #1a1c2a; + min-height: 100vh; + padding: 12px; + color: #dfe3e8; +} + +::selection { + background: rgba(34, 211, 238, 0.25); + color: #fff; +} + +.container { + max-width: 1600px; + margin: 0 auto; +} + +/* --- Header --- */ + +header { + background: rgba(255, 255, 255, 0.04); + backdrop-filter: blur(12px); + -webkit-backdrop-filter: blur(12px); + border-radius: 0; + padding: 12px 16px; + margin-bottom: 12px; + border: 1px solid rgba(255, 255, 255, 0.06); +} + +header h1 { + font-size: 16px; + font-weight: 700; + margin-bottom: 10px; + color: #fff; + letter-spacing: 0.5px; +} + +.connection-controls { + display: flex; + gap: 6px; + align-items: center; + flex-wrap: wrap; +} + +.port-selector { + display: flex; + gap: 4px; + align-items: center; +} + +#port-select { + padding: 5px 10px; + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: 0; + font-size: 11px; + font-family: 'Space Mono', monospace; + min-width: 180px; + max-width: 300px; + background: rgba(255, 255, 255, 0.04); + color: #dfe3e8; + outline: none; + transition: border-color 0.15s; +} + +#port-select:focus { + border-color: rgba(34, 211, 238, 0.4); +} + +#mode-select { + background: rgba(255, 255, 255, 0.04); + color: #dfe3e8; + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: 0; + padding: 5px 10px; + font-size: 11px; + font-family: 'Space Mono', monospace; + outline: none; + transition: border-color 0.15s; +} + +#mode-select:focus { + border-color: rgba(34, 211, 238, 0.4); +} + +/* --- Buttons --- */ + +.btn { + padding: 5px 10px; + border: 1px solid transparent; + border-radius: 0; + font-size: 11px; + font-weight: 600; + cursor: pointer; + transition: all 0.15s; + font-family: 'Space Mono', monospace; + letter-spacing: 0.3px; +} + +.btn:disabled { + opacity: 0.35; + cursor: not-allowed; +} + +.btn-primary { + background: rgba(34, 211, 238, 0.12); + color: #22d3ee; + border-color: rgba(34, 211, 238, 0.25); +} + +.btn-primary:hover:not(:disabled) { + background: rgba(34, 211, 238, 0.2); + border-color: rgba(34, 211, 238, 0.4); +} + +.btn-secondary { + background: rgba(255, 255, 255, 0.04); + color: #9faab9; + border-color: rgba(255, 255, 255, 0.08); +} + +.btn-secondary:hover:not(:disabled) { + background: rgba(255, 255, 255, 0.08); + color: #dfe3e8; +} + +.btn-info { + background: rgba(16, 185, 129, 0.12); + color: #34d399; + border-color: rgba(16, 185, 129, 0.25); +} + +.btn-info:hover:not(:disabled) { + background: rgba(16, 185, 129, 0.2); + border-color: rgba(16, 185, 129, 0.4); +} + +.btn-scan { + background: rgba(139, 92, 246, 0.12); + color: #a78bfa; + border-color: rgba(139, 92, 246, 0.25); + font-size: 10px; + padding: 5px 8px; +} + +.btn-scan:hover:not(:disabled) { + background: rgba(139, 92, 246, 0.2); + border-color: rgba(139, 92, 246, 0.4); +} + +.status-badge { + padding: 4px 10px; + background: rgba(34, 211, 238, 0.08); + color: #22d3ee; + border: 1px solid rgba(34, 211, 238, 0.15); + border-radius: 0; + font-size: 10px; + font-weight: 600; + display: none; + letter-spacing: 0.3px; +} + +/* --- Status Panel --- */ + +.status-panel { + display: grid; + grid-template-columns: repeat(auto-fit, minmax(180px, 1fr)); + gap: 8px; + margin-bottom: 12px; +} + +.status-card { + background: rgba(255, 255, 255, 0.03); + backdrop-filter: blur(8px); + -webkit-backdrop-filter: blur(8px); + border-radius: 0; + padding: 10px 12px; + border: 1px solid rgba(255, 255, 255, 0.06); +} + +.status-card h3 { + font-size: 10px; + color: #7f8ea2; + margin-bottom: 5px; + text-transform: uppercase; + letter-spacing: 0.8px; +} + +.status-card div:not(h3) { + font-size: 12px; + color: #dfe3e8; +} + +.status-indicator { + font-size: 12px; + font-weight: 600; + padding: 3px 8px; + border-radius: 0; + display: inline-block; +} + +.status-indicator.connected { + background: rgba(16, 185, 129, 0.12); + color: #34d399; + border: 1px solid rgba(16, 185, 129, 0.2); +} + +.status-indicator.disconnected { + background: rgba(200, 100, 100, 0.1); + color: #d4878a; + border: 1px solid rgba(200, 100, 100, 0.15); +} + +/* --- Sensors Panel --- */ + +.sensors-panel, .forces-panel { + background: rgba(255, 255, 255, 0.03); + backdrop-filter: blur(8px); + -webkit-backdrop-filter: blur(8px); + border-radius: 0; + padding: 12px; + margin-bottom: 12px; + border: 1px solid rgba(255, 255, 255, 0.06); +} + +.sensors-panel h2, .forces-panel h2 { + font-size: 13px; + font-weight: 700; + margin-bottom: 10px; + color: #fff; + letter-spacing: 0.3px; +} + +.sensors-grid { + display: grid; + grid-template-columns: repeat(auto-fit, minmax(120px, 1fr)); + gap: 6px; +} + +.sensor-card { + background: rgba(255, 255, 255, 0.03); + border: 1px solid rgba(255, 255, 255, 0.06); + border-radius: 0; + padding: 8px; + text-align: center; + transition: all 0.15s; +} + +.sensor-card:hover { + border-color: rgba(255, 255, 255, 0.12); + background: rgba(255, 255, 255, 0.05); +} + +.sensor-name { + font-size: 11px; + font-weight: 600; + margin-bottom: 4px; + color: #dfe3e8; + letter-spacing: 0.3px; +} + +.sensor-status { + font-size: 18px; + margin-bottom: 4px; + line-height: 1; +} + +.sensor-status.connected { + color: #34d399; +} + +.sensor-status.disconnected { + color: #d4878a; +} + +.sensor-taxels { + font-size: 10px; + color: #7f8ea2; +} + +.sensor-taxels span { + font-weight: 600; + color: #dfe3e8; +} + +/* --- Forces Panel --- */ + +.forces-container { + display: grid; + grid-template-columns: repeat(auto-fit, minmax(260px, 1fr)); + gap: 10px; +} + +.force-visualization { + background: rgba(255, 255, 255, 0.02); + border: 1px solid rgba(255, 255, 255, 0.06); + border-radius: 0; + padding: 16px; + text-align: center; +} + +.force-label { + font-size: 14px; + font-weight: 700; + margin-bottom: 12px; + color: #fff; + letter-spacing: 0.3px; +} + +.force-arrow { + width: 100%; + height: 200px; + margin: 12px 0; + position: relative; +} + +.force-arrow circle { + transition: cx 0.05s ease-out, cy 0.05s ease-out, r 0.05s ease-out, opacity 0.05s ease-out; +} + +.force-values { + display: grid; + grid-template-columns: repeat(2, 1fr); + gap: 6px; + font-size: 12px; + color: #7f8ea2; +} + +.force-values span { + font-weight: 600; + color: #dfe3e8; +} + +.force-magnitude { + grid-column: 1 / -1; + font-size: 13px; + font-weight: 600; + color: #22d3ee; + margin-top: 6px; + padding-top: 6px; + border-top: 1px solid rgba(255, 255, 255, 0.06); +} + +/* --- Taxels Panel --- */ + +.taxels-panel { + background: rgba(255, 255, 255, 0.03); + backdrop-filter: blur(8px); + -webkit-backdrop-filter: blur(8px); + border-radius: 0; + padding: 12px; + margin-bottom: 12px; + border: 1px solid rgba(255, 255, 255, 0.06); +} + +.taxels-header { + display: flex; + justify-content: space-between; + align-items: center; + margin-bottom: 10px; + flex-wrap: wrap; + gap: 8px; +} + +.taxels-header h2 { + font-size: 13px; + font-weight: 700; + color: #fff; + margin: 0; + letter-spacing: 0.3px; +} + +/* --- Taxel Controls Toolbar --- */ + +.taxel-controls { + display: flex; + align-items: center; + gap: 0; + flex-wrap: wrap; + background: rgba(255, 255, 255, 0.03); + border: 1px solid rgba(255, 255, 255, 0.06); + padding: 2px; +} + +.toggle-label { + display: flex; + align-items: center; + gap: 4px; + font-size: 10px; + cursor: pointer; + padding: 5px 10px; + background: transparent; + color: #7f8ea2; + border: none; + border-right: 1px solid rgba(255, 255, 255, 0.06); + border-radius: 0; + transition: all 0.15s; +} + +.toggle-label:last-child { + border-right: none; +} + +.toggle-label:hover { + background: rgba(255, 255, 255, 0.04); + color: #9faab9; +} + +.toggle-label:has(input:checked) { + background: rgba(34, 211, 238, 0.1); + color: #22d3ee; +} + +.toggle-label input[type="checkbox"], +.toggle-label input[type="radio"] { + display: none; +} + +.toggle-label input[type="checkbox"] { + display: inline-block; + width: 12px; + height: 12px; + cursor: pointer; + margin: 0; + accent-color: #22d3ee; +} + +.color-legend { + font-size: 9px; + color: #5f718b; + padding: 5px 10px; + border-right: none; +} + +.magnitude-legend, +.direction-legend, +.arrows-legend { + display: none; +} + +.direction-legend { + display: inline-flex; + gap: 4px; + align-items: center; +} + +.direction-legend span { + padding: 2px 6px; + margin-right: 0; + font-weight: 600; + font-size: 9px; + border: 1px solid; +} + +.dir-right { background: rgba(200, 100, 100, 0.1); color: #d4878a; border-color: rgba(200, 100, 100, 0.2) !important; } +.dir-left { background: rgba(6, 182, 212, 0.12); color: #22d3ee; border-color: rgba(6, 182, 212, 0.2) !important; } +.dir-up { background: rgba(16, 185, 129, 0.12); color: #34d399; border-color: rgba(16, 185, 129, 0.2) !important; } +.dir-down { background: rgba(200, 160, 80, 0.12); color: #c8a870; border-color: rgba(200, 160, 80, 0.2) !important; } + +/* --- Row Labels --- */ + +.viz-row-label { + font-size: 10px; + font-weight: 700; + color: #5f718b; + text-transform: uppercase; + letter-spacing: 1px; + padding: 6px 0 4px 0; +} + +/* --- Taxel Grids (2D) --- */ + +.taxels-container { + display: grid; + grid-template-columns: repeat(5, 1fr); + gap: 10px; +} + +.taxel-finger { + background: #000000; + border: 1px solid rgba(255, 255, 255, 0.06); + border-radius: 0; + padding: 10px; + text-align: center; + aspect-ratio: 1 / 1; + display: flex; + flex-direction: column; + align-items: center; + justify-content: center; +} + +.taxel-finger-label { + font-size: 12px; + font-weight: 600; + margin-bottom: 8px; + color: #dfe3e8; + letter-spacing: 0.3px; +} + +.taxel-grid { + display: flex; + flex-direction: column; + align-items: center; + gap: 1px; +} + +.taxel-row { + display: flex; + gap: 1px; + justify-content: center; +} + +.taxel-cell { + width: 10px; + height: 10px; + border-radius: 0; + background: #1a1a1a; + transition: background-color 0.05s ease-out; +} + +.taxel-cell.inactive { + visibility: hidden; +} + +.taxel-cell.dir-right { background: #c47070; } +.taxel-cell.dir-left { background: #06b6d4; } +.taxel-cell.dir-up { background: #10b981; } +.taxel-cell.dir-down { background: #c8a050; } + +/* --- SVG Taxels --- */ + +.taxel-svg { + display: block; + margin: 0 auto; +} + +.taxel-circle { + transition: fill 0.05s ease-out; +} + +.taxel-arrow { + pointer-events: none; +} + +.taxel-arrow line, +.taxel-arrow polygon { + transition: stroke 0.05s ease-out, fill 0.05s ease-out; +} + +.view-separator { + display: none; +} + +/* --- Slider Controls --- */ + +.slider-control input[type="range"] { + width: 56px; + height: 3px; + cursor: pointer; + accent-color: #22d3ee; +} + +.slider-control span:last-child { + min-width: 26px; + font-size: 9px; + text-align: right; + color: #7f8ea2; +} + +#color-scheme-select { + background: rgba(255, 255, 255, 0.04); + color: #9faab9; + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: 0; + font-size: 9px; + font-family: 'Space Mono', monospace; + padding: 2px 6px; + cursor: pointer; + outline: none; +} + +#color-scheme-select:focus { + border-color: rgba(34, 211, 238, 0.3); +} + +#threshold-input { + width: 44px; + background: rgba(255, 255, 255, 0.04); + color: #9faab9; + border: 1px solid rgba(255, 255, 255, 0.08); + border-radius: 0; + font-size: 9px; + font-family: 'Space Mono', monospace; + padding: 2px 6px; + text-align: center; + outline: none; +} + +#threshold-input:focus { + border-color: rgba(34, 211, 238, 0.3); +} + +#threshold-input:disabled { + opacity: 0.3; + cursor: not-allowed; +} + +/* --- Error Panel --- */ + +.error-panel { + background: rgba(200, 100, 100, 0.08); + border: 1px solid rgba(200, 100, 100, 0.2); + border-radius: 0; + padding: 10px; + margin-top: 12px; +} + +.error-message { + color: #d4878a; + font-weight: 600; + font-size: 11px; +} diff --git a/scripts/tactile_sensing_ui/tactile_ui.py b/scripts/tactile_sensing_ui/tactile_ui.py new file mode 100755 index 00000000..a59efbc3 --- /dev/null +++ b/scripts/tactile_sensing_ui/tactile_ui.py @@ -0,0 +1,341 @@ +#!/usr/bin/env python3 +"""Web-based testing UI for ORCA Sensor Client""" + +from flask import Flask, render_template, jsonify, request +from flask_socketio import SocketIO, emit +import sys +import os +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from orca_core.hardware.sensing.sensor_client import SensorClient +from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates +from orca_core.utils.utils import read_yaml, update_yaml +import argparse +import yaml +import serial.tools.list_ports +import threading +import time + +SENSOR_ADAPTER_VID = 0x28E9 +SENSOR_ADAPTER_PID = 0x018A + +app = Flask(__name__) +app.config['SECRET_KEY'] = 'orca_sensor_secret' +socketio = SocketIO(app, cors_allowed_origins="*", async_mode='threading') + +sensor_client = None +stream_thread = None +stream_thread_running = False +current_mode = 'resultant' # 'resultant', 'taxels', or 'combined' +finger_to_sensor_id_config = None # Loaded from --config if provided +config_dir = None # Set from --config arg directory, for calibration.yaml access + +def get_sensor_client(): + global sensor_client + if sensor_client is None: + port = request.args.get('port', '/dev/ttyACM0') + sensor_client = SensorClient(port=port) + return sensor_client + +def stream_update_loop(): + """Background thread that reads from auto-stream and emits via websocket.""" + global stream_thread_running, sensor_client, current_mode + + while stream_thread_running: + try: + if sensor_client and sensor_client.is_connected: + if current_mode == 'resultant': + forces, ts = sensor_client.get_auto_latest() + if forces is not None: + socketio.emit('force_update', forces) + elif current_mode == 'taxels': + taxels, ts = sensor_client.get_auto_latest_taxels() + if taxels is not None: + socketio.emit('taxel_update', taxels) + elif current_mode == 'combined': + forces, taxels, ts = sensor_client.get_auto_latest_all() + if forces is not None or taxels is not None: + socketio.emit('combined_update', { + 'forces': forces, + 'taxels': taxels + }) + time.sleep(0.01) # ~100Hz update rate + except Exception as e: + socketio.emit('error', {'message': str(e)}) + time.sleep(0.1) + +def start_stream(mode): + """Start auto-stream with specified mode.""" + global sensor_client, stream_thread, stream_thread_running, current_mode + + # Stop existing stream + stop_stream() + + current_mode = mode + + # Configure and start auto-stream + if mode == 'resultant': + sensor_client.start_auto_stream(resultant=True, taxels=False) + elif mode == 'taxels': + sensor_client.start_auto_stream(resultant=False, taxels=True) + elif mode == 'combined': + sensor_client.start_auto_stream(resultant=True, taxels=True) + + # Start websocket emission thread + stream_thread_running = True + stream_thread = threading.Thread(target=stream_update_loop, daemon=True) + stream_thread.start() + +def stop_stream(): + """Stop auto-stream and emission thread.""" + global sensor_client, stream_thread, stream_thread_running + + stream_thread_running = False + if stream_thread: + stream_thread.join(timeout=1) + stream_thread = None + + if sensor_client and sensor_client.is_connected: + try: + sensor_client.stop_auto_stream() + except Exception: + pass + +@app.route('/') +def index(): + return render_template('index.html') + +@app.route('/api/ports') +def list_ports(): + ports = serial.tools.list_ports.comports() + result = [] + for p in ports: + is_sensor = (p.vid == SENSOR_ADAPTER_VID and p.pid == SENSOR_ADAPTER_PID) + if p.vid is not None: + result.append({ + 'device': p.device, + 'description': p.description, + 'is_sensor_adapter': is_sensor, + }) + result.sort(key=lambda x: (not x['is_sensor_adapter'], x['device'])) + return jsonify(result) + +@app.route('/api/connect', methods=['POST']) +def connect(): + try: + data = request.json + port = data.get('port', '/dev/ttyACM0') + mode = data.get('mode', 'resultant') + global sensor_client, current_mode + + if sensor_client and sensor_client.is_connected: + stop_stream() + sensor_client.disconnect() + + sensor_client = SensorClient(port=port, finger_to_sensor_id=finger_to_sensor_id_config) + sensor_client.connect() + + # Load saved sensor offsets if config was provided + if config_dir: + calib_path = os.path.join(config_dir, 'calibration.yaml') + calib_data = read_yaml(calib_path) + if calib_data and 'sensor_offsets' in calib_data: + sensor_client.set_taxel_offsets(calib_data['sensor_offsets']) + + # Start streaming with requested mode + start_stream(mode) + + # Get configuration for response + config = sensor_client.get_sensor_configuration() + + socketio.emit('connection_status', {'connected': True, 'mode': mode}) + return jsonify({ + 'success': True, + 'message': f'Connected to {port}', + 'mode': mode, + 'config': { + 'active_sensors': config.active_sensors if config else [], + 'num_taxels': config.num_taxels if config else {} + } + }) + except Exception as e: + socketio.emit('connection_status', {'connected': False, 'error': str(e)}) + return jsonify({'success': False, 'message': str(e)}), 400 + +@app.route('/api/disconnect', methods=['POST']) +def disconnect(): + try: + global sensor_client + stop_stream() + if sensor_client and sensor_client.is_connected: + sensor_client.disconnect() + socketio.emit('connection_status', {'connected': False}) + return jsonify({'success': True, 'message': 'Disconnected'}) + except Exception as e: + return jsonify({'success': False, 'message': str(e)}), 400 + +@app.route('/api/mode', methods=['POST']) +def set_mode(): + """Change the streaming mode.""" + try: + global sensor_client, current_mode + data = request.json + mode = data.get('mode', 'resultant') + + if mode not in ('resultant', 'taxels', 'combined'): + return jsonify({'success': False, 'message': f'Invalid mode: {mode}'}), 400 + + if not sensor_client or not sensor_client.is_connected: + return jsonify({'success': False, 'message': 'Not connected'}), 400 + + start_stream(mode) + socketio.emit('mode_changed', {'mode': mode}) + return jsonify({'success': True, 'mode': mode}) + except Exception as e: + return jsonify({'success': False, 'message': str(e)}), 400 + +@app.route('/api/zero', methods=['POST']) +def zero(): + """Capture current sensor readings as zero baseline.""" + try: + global sensor_client + if not sensor_client or not sensor_client.is_connected: + return jsonify({'success': False, 'message': 'Not connected'}), 400 + + offsets = sensor_client.capture_taxel_offsets(num_samples=100) + + if config_dir: + calib_path = os.path.join(config_dir, 'calibration.yaml') + update_yaml(calib_path, 'sensor_offsets', offsets) + + return jsonify({'success': True, 'message': 'Sensor offsets captured and applied'}) + except Exception as e: + return jsonify({'success': False, 'message': str(e)}), 400 + +@app.route('/api/clear_zero', methods=['POST']) +def clear_zero(): + """Clear sensor zeroing offsets.""" + try: + global sensor_client + if not sensor_client or not sensor_client.is_connected: + return jsonify({'success': False, 'message': 'Not connected'}), 400 + + sensor_client.clear_taxel_offsets() + return jsonify({'success': True, 'message': 'Sensor offsets cleared'}) + except Exception as e: + return jsonify({'success': False, 'message': str(e)}), 400 + +@app.route('/api/status') +def status(): + client = get_sensor_client() + if not client.is_connected: + return jsonify({'connected': False}) + + out = {'connected': True, 'mode': current_mode} + errors = [] + + try: + out['hardware_version'] = client.read_hardware_version() + except Exception as e: + errors.append(f"read_hardware_version: {e}") + + try: + out['sensors'] = client.read_connected_sensors() + except Exception as e: + errors.append(f"read_connected_sensors: {e}") + + try: + out['taxels'] = client.read_num_taxels() + except Exception as e: + errors.append(f"read_num_taxels: {e}") + + try: + out['auto_data_type'] = client.read_auto_data_type() + except Exception as e: + errors.append(f"read_auto_data_type: {e}") + + # Get stream stats + try: + stats = client.get_auto_stats() + out['stream_stats'] = { + 'frames_ok': stats.frames_ok, + 'parse_ok': stats.parse_ok, + 'parse_errors': stats.parse_errors + } + except Exception: + pass + + out['status_ok'] = (len(errors) == 0) + if errors: + out['errors'] = errors + + return jsonify(out) + + +@app.route('/api/forces') +def forces(): + try: + client = get_sensor_client() + if not client.is_connected: + return jsonify({'error': 'Not connected'}), 400 + forces = client.read_resulting_force() + return jsonify(forces) + except Exception as e: + return jsonify({'error': str(e)}), 400 + +@app.route('/api/taxel_coordinates') +def taxel_coordinates(): + """Return taxel coordinates for all fingers.""" + return jsonify(get_all_coordinates()) + + +@app.route('/api/refresh') +def refresh(): + try: + client = get_sensor_client() + if not client.is_connected: + return jsonify({'connected': False}) + + connected = client.read_connected_sensors() + version = client.read_hardware_version() + taxels = client.read_num_taxels() + auto_data = client.read_auto_data_type() + forces = client.read_resulting_force() + + return jsonify({ + 'connected': True, + 'hardware_version': version, + 'sensors': connected, + 'taxels': taxels, + 'auto_data_type': auto_data, + 'forces': forces, + 'mode': current_mode + }) + except Exception as e: + return jsonify({'connected': False, 'error': str(e)}), 400 + +@socketio.on('connect') +def handle_connect(): + emit('connected', {'data': 'Connected to WebSocket'}) + +@socketio.on('disconnect') +def handle_disconnect(): + pass + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='ORCA Sensor Testing UI') + parser.add_argument('--config', type=str, default=None, + help='Path to hand config YAML (for sensor wiring mapping)') + args = parser.parse_args() + + if args.config: + config_dir = os.path.dirname(os.path.abspath(args.config)) + with open(args.config) as f: + config_data = yaml.safe_load(f) + sensors_cfg = config_data.get('sensors', {}) + mapping = sensors_cfg.get('finger_to_sensor_id') + if mapping: + finger_to_sensor_id_config = mapping + print(f"Loaded sensor mapping from {args.config}: {finger_to_sensor_id_config}") + + socketio.run(app, host='0.0.0.0', port=5001, debug=True, allow_unsafe_werkzeug=True) diff --git a/scripts/tactile_sensing_ui/templates/index.html b/scripts/tactile_sensing_ui/templates/index.html new file mode 100644 index 00000000..d77cff3f --- /dev/null +++ b/scripts/tactile_sensing_ui/templates/index.html @@ -0,0 +1,230 @@ + + + + + + ORCA Sensor Testing UI + + + + + + + +
+
+

ORCA Sensor Testing UI

+
+
+ + +
+ + + + + + +
Streaming: 100Hz
+
+
+ +
+
+

Connection Status

+
Disconnected
+
+
+

Hardware Version

+
-
+
+
+

Stream Mode

+
-
+
+
+

Auto Data Type

+
-
+
+
+ +
+

Sensor Status

+
+
+
Thumb
+
*
+
Taxels: -
+
+
+
Index
+
*
+
Taxels: -
+
+
+
Middle
+
*
+
Taxels: -
+
+
+
Ring
+
*
+
Taxels: -
+
+
+
Pinky
+
*
+
Taxels: -
+
+
+
+ + + + + +
+
+

Taxel Visualization

+
+ + + + | + + | + + + | + +
+ + + Right + Up + Left + Down + + +
+
+
+
+ +
+
+ + +
+ + + + diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py new file mode 100644 index 00000000..7475cd25 --- /dev/null +++ b/tests/test_tactile_sensor.py @@ -0,0 +1,381 @@ +"""Tests for tactile sensor data contracts and parsing. + +Validates byte formats, data shapes, payload sizes, and protocol details +without requiring hardware. Uses MockSensorClient and direct parser calls. +""" + +import struct +import time + +import pytest + +from orca_core.hardware.sensing.sensor_client import ( + SensorClient, + SensorConfiguration, + calculate_checksum, + FINGER_NAMES, +) +from orca_core.hardware.sensing.mock_sensor_client import MockSensorClient +from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates + +EXPECTED_TAXEL_COUNTS = { + "thumb": 51, "index": 87, "middle": 87, "ring": 87, "pinky": 51, +} + +ALL_FINGERS = ["thumb", "index", "middle", "ring", "pinky"] + + +def _make_config( + connected_fingers: list[str], + taxel_counts: dict[str, int] | None = None, + finger_to_sensor_id: dict[str, int] | None = None, +) -> SensorConfiguration: + """Build a SensorConfiguration for testing.""" + if taxel_counts is None: + taxel_counts = EXPECTED_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: finger_to_sensor_id[f] * 4 + 2 for f in connected_fingers} + + num_active = len(connected_fingers) + expected_resultant = num_active * 6 + expected_taxels = sum(num_taxels[f] * 3 for f in connected_fingers) + + return SensorConfiguration( + connected=connected, + num_taxels=num_taxels, + module_indices=module_indices, + expected_payload_size_resultant=expected_resultant, + expected_payload_size_taxels=expected_taxels, + expected_payload_size_combined=expected_resultant + expected_taxels, + timestamp=time.time(), + finger_to_sensor_id=finger_to_sensor_id, + ) + + +def _sensor_client_instance() -> SensorClient: + """Create a SensorClient without connecting (for calling parse methods).""" + client = SensorClient.__new__(SensorClient) + return client + + +# --------------------------------------------------------------------------- +# Test 1: Mock client — resultant forces shape +# --------------------------------------------------------------------------- + +class TestMockResultantForces: + def test_shape_all_fingers(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() + forces = {f: [1.0, -0.5, 2.0] for f in ALL_FINGERS} + mock.set_mock_forces(forces) + mock.start_auto_stream(resultant=True, taxels=False) + time.sleep(0.05) + + result, ts = mock.get_auto_latest() + mock.stop_auto_stream() + mock.disconnect() + + assert result is not None + assert ts is not None + assert set(result.keys()) == set(ALL_FINGERS) + for finger in ALL_FINGERS: + assert len(result[finger]) == 3 + assert all(isinstance(v, float) for v in result[finger]) + + def test_values_match_no_noise(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.set_noise_level(0.0) + mock.connect() + mock.set_mock_forces({"thumb": [1.5, -2.0, 3.0], "index": [0.0, 0.0, 0.0]}) + mock.start_auto_stream(resultant=True, taxels=False) + time.sleep(0.05) + + result, _ = mock.get_auto_latest() + mock.stop_auto_stream() + mock.disconnect() + + assert result["thumb"] == [1.5, -2.0, 3.0] + assert result["index"] == [0.0, 0.0, 0.0] + + def test_subset_of_fingers(self): + subset = ["thumb", "pinky"] + mock = MockSensorClient(connected_sensors=subset) + mock.connect() + mock.set_mock_forces({"thumb": [1.0, 0.0, 0.0], "pinky": [0.0, 1.0, 0.0]}) + mock.start_auto_stream(resultant=True, taxels=False) + time.sleep(0.05) + + result, _ = mock.get_auto_latest() + mock.stop_auto_stream() + mock.disconnect() + + assert set(result.keys()) == set(subset) + + +# --------------------------------------------------------------------------- +# Test 2: Mock client — taxel data shape +# --------------------------------------------------------------------------- + +class TestMockTaxelData: + def test_shape_all_fingers(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() + mock.start_auto_stream(resultant=False, taxels=True) + time.sleep(0.05) + + result, ts = mock.get_auto_latest_taxels() + mock.stop_auto_stream() + mock.disconnect() + + assert result is not None + assert set(result.keys()) == set(ALL_FINGERS) + for finger in ALL_FINGERS: + assert len(result[finger]) == EXPECTED_TAXEL_COUNTS[finger], ( + f"{finger}: expected {EXPECTED_TAXEL_COUNTS[finger]} taxels, " + f"got {len(result[finger])}" + ) + for taxel in result[finger]: + assert len(taxel) == 3 + assert all(isinstance(v, float) for v in taxel) + + +# --------------------------------------------------------------------------- +# Test 3: Mock client — combined mode shape +# --------------------------------------------------------------------------- + +class TestMockCombinedMode: + def test_shape(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() + mock.set_mock_forces({f: [1.0, 0.0, 0.5] for f in ALL_FINGERS}) + mock.start_auto_stream(resultant=True, taxels=True) + time.sleep(0.05) + + forces, taxels, ts = mock.get_auto_latest_all() + mock.stop_auto_stream() + mock.disconnect() + + assert forces is not None + assert taxels is not None + assert set(forces.keys()) == set(ALL_FINGERS) + assert set(taxels.keys()) == set(ALL_FINGERS) + for finger in ALL_FINGERS: + assert len(forces[finger]) == 3 + assert len(taxels[finger]) == EXPECTED_TAXEL_COUNTS[finger] + + +# --------------------------------------------------------------------------- +# Test 4: Payload size calculation +# --------------------------------------------------------------------------- + +class TestPayloadSize: + def test_all_sensors(self): + config = _make_config(ALL_FINGERS) + total_taxels = 51 + 87 + 87 + 87 + 51 # 363 + assert config.expected_payload_size_resultant == 5 * 6 # 30 + assert config.expected_payload_size_taxels == total_taxels * 3 # 1089 + assert config.expected_payload_size_combined == 30 + total_taxels * 3 # 1119 + + def test_two_sensors(self): + config = _make_config(["thumb", "index"]) + assert config.expected_payload_size_resultant == 2 * 6 # 12 + assert config.expected_payload_size_taxels == (51 + 87) * 3 # 414 + assert config.expected_payload_size_combined == 12 + 414 # 426 + + def test_single_sensor(self): + config = _make_config(["pinky"]) + assert config.expected_payload_size_resultant == 6 + assert config.expected_payload_size_taxels == 51 * 3 # 153 + assert config.expected_payload_size_combined == 6 + 153 # 159 + + def test_no_sensors(self): + config = _make_config([]) + assert config.expected_payload_size_resultant == 0 + assert config.expected_payload_size_taxels == 0 + assert config.expected_payload_size_combined == 0 + + +# --------------------------------------------------------------------------- +# Test 5: Parse resultant compact — known bytes +# --------------------------------------------------------------------------- + +class TestParseResultantCompact: + def test_known_values(self): + client = _sensor_client_instance() + config = _make_config(["thumb"]) + + # fx=100 (10.0N), fy=-50 (-5.0N), fz=200 (20.0N) + data = struct.pack(" Date: Sat, 28 Mar 2026 20:01:48 +0100 Subject: [PATCH 02/20] Created constants file, moved sensor clients into hardware folder and moved constants into new constants file --- orca_core/hand_config.py | 19 +++--- .../{sensing => }/mock_sensor_client.py | 27 +++----- orca_core/hardware/sensing/constants.py | 46 +++++++++++++ .../sensing/models/sensor_models.yaml | 15 ----- .../hardware/sensing/taxel_coordinates.py | 25 ++----- .../hardware/{sensing => }/sensor_client.py | 66 ++++++++----------- orca_core/hardware_hand.py | 2 +- scripts/tactile_sensing_ui/tactile_ui.py | 2 +- tests/test_tactile_sensor.py | 4 +- 9 files changed, 104 insertions(+), 102 deletions(-) rename orca_core/hardware/{sensing => }/mock_sensor_client.py (97%) create mode 100644 orca_core/hardware/sensing/constants.py delete mode 100644 orca_core/hardware/sensing/models/sensor_models.yaml rename orca_core/hardware/{sensing => }/sensor_client.py (98%) diff --git a/orca_core/hand_config.py b/orca_core/hand_config.py index 147a0615..e86cecd3 100644 --- a/orca_core/hand_config.py +++ b/orca_core/hand_config.py @@ -12,6 +12,13 @@ from typing import Dict, List, Literal from .constants import CONTROL_MODES, DEFAULT_MODEL_NAME, JOINT_IDS, JOINT_ROM_DICT, JOINT_TO_MOTOR_MAP, MOTOR_IDS +from .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 @@ -323,20 +330,14 @@ def __post_init__(self) -> None: self.validate_config() -FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] -VALID_SENSOR_IDS = set(range(5)) - - @dataclass(frozen=True) class OrcaHandTouchConfig(OrcaHandConfig): """ORCA hand configuration with tactile sensor support.""" - sensor_port: str = "/dev/ttyACM0" - sensor_baudrate: int = 921600 + sensor_port: str = DEFAULT_SENSOR_PORT + sensor_baudrate: int = DEFAULT_SENSOR_BAUDRATE finger_to_sensor_id: Dict[str, int] = field( - default_factory=lambda: { - "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4, - } + default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID) ) @classmethod diff --git a/orca_core/hardware/sensing/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py similarity index 97% rename from orca_core/hardware/sensing/mock_sensor_client.py rename to orca_core/hardware/mock_sensor_client.py index 281d8925..9a79724f 100644 --- a/orca_core/hardware/sensing/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -14,19 +14,14 @@ import random import logging -logger = logging.getLogger(__name__) - -FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] +from orca_core.hardware.sensing.constants import ( + FINGER_NAMES, + DEFAULT_FINGER_TO_SENSOR_ID, + DEFAULT_TAXEL_COUNTS, + DEFAULT_SENSOR_BAUDRATE, +) -# Default taxel counts loaded from sensor model config -def _get_default_taxel_counts() -> dict[str, int]: - try: - from orca_core.hardware.sensing.taxel_coordinates import get_taxel_counts - return get_taxel_counts() - except Exception: - return {"thumb": 51, "index": 87, "middle": 87, "ring": 87, "pinky": 51} - -DEFAULT_TAXEL_COUNTS = _get_default_taxel_counts() +logger = logging.getLogger(__name__) class NoSensorsAvailableError(Exception): @@ -58,9 +53,7 @@ class SensorConfiguration: expected_payload_size_taxels: int = 0 expected_payload_size_combined: int = 0 timestamp: float = 0.0 - finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: { - "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 - }) + finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID)) @property def active_sensors(self) -> list[str]: @@ -96,8 +89,8 @@ class MockSensorClient: """ def __init__(self, - port: str = '/dev/ttyUSB0', - baudrate: int = 921600, + port: str = '', + baudrate: int = DEFAULT_SENSOR_BAUDRATE, connected_sensors: Optional[list[str]] = None, finger_to_sensor_id: Optional[dict[str, int]] = None): """Initialize mock sensor client. diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py new file mode 100644 index 00000000..c79376cd --- /dev/null +++ b/orca_core/hardware/sensing/constants.py @@ -0,0 +1,46 @@ +"""Constants for ORCA tactile sensing.""" + +FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] +VALID_SENSOR_IDS = set(range(5)) + +# Default hardware settings +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-to-sensor-model mapping (replaces sensor_models.yaml) +FINGER_MODELS = { + "thumb": "touch-sensor-thumb", + "index": "touch-sensor-finger", + "middle": "touch-sensor-finger", + "ring": "touch-sensor-finger", + "pinky": "touch-sensor-pinky", +} + +# Protocol constants +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 +ADDR_HARDWARE_VERSION_START = 0x0000 +ADDR_HARDWARE_VERSION_LENGTH = 16 +ADDR_RESET = 0x0022 +ADDR_CONNECTED_SENSORS_START = 0x0010 +ADDR_CONNECTED_SENSORS_LENGTH = 4 +ADDR_NUM_TAXELS_START = 0x0030 +ADDR_NUM_TAXELS_LENGTH = 56 +ADDR_RESULTING_FORCE_START = 0x0500 +ADDR_RESULTING_FORCE_LENGTH = 168 +ADDR_AUTO_DATA_TYPE = 0x0016 +ADDR_AUTO_ENABLE = 0x0017 diff --git a/orca_core/hardware/sensing/models/sensor_models.yaml b/orca_core/hardware/sensing/models/sensor_models.yaml deleted file mode 100644 index 0fb3a634..00000000 --- a/orca_core/hardware/sensing/models/sensor_models.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# Finger-to-sensor-model mapping -# Change these to switch which sensor model is used on each finger. -# Model names must match a subdirectory in this folder. -# -# Available models: -# touch-sensor-thumb - ORCA Fingertip Thumb (51 taxels) -# touch-sensor-finger - ORCA Fingertip Finger (87 taxels) -# touch-sensor-pinky - ORCA Fingertip Pinky (51 taxels) - -finger_models: - thumb: touch-sensor-thumb - index: touch-sensor-finger - middle: touch-sensor-finger - ring: touch-sensor-finger - pinky: touch-sensor-pinky diff --git a/orca_core/hardware/sensing/taxel_coordinates.py b/orca_core/hardware/sensing/taxel_coordinates.py index c0785910..1d9008c7 100644 --- a/orca_core/hardware/sensing/taxel_coordinates.py +++ b/orca_core/hardware/sensing/taxel_coordinates.py @@ -5,9 +5,11 @@ """ import os -from typing import TypedDict, Optional +from typing import TypedDict import yaml +from orca_core.hardware.sensing.constants import FINGER_MODELS + class TaxelCoord(TypedDict): x: float @@ -16,23 +18,8 @@ class TaxelCoord(TypedDict): MODELS_DIR = os.path.join(os.path.dirname(__file__), "models") -SENSOR_MODELS_CONFIG = os.path.join(MODELS_DIR, "sensor_models.yaml") _model_cache: dict[str, list[TaxelCoord]] = {} -_finger_mapping_cache: Optional[dict[str, str]] = None - - -def _load_finger_mapping() -> dict[str, str]: - """Load the finger-to-model mapping from sensor_models.yaml.""" - global _finger_mapping_cache - if _finger_mapping_cache is not None: - return _finger_mapping_cache - - with open(SENSOR_MODELS_CONFIG, "r") as f: - config = yaml.safe_load(f) - - _finger_mapping_cache = config["finger_models"] - return _finger_mapping_cache def _load_model_coordinates(model_name: str) -> list[TaxelCoord]: @@ -58,7 +45,7 @@ def get_coordinates(finger: str) -> list[TaxelCoord]: Returns: List of coordinate dicts with 'x', 'y', 'z' keys (in mm) """ - mapping = _load_finger_mapping() + mapping = FINGER_MODELS model_name = mapping.get(finger) if model_name is None: return [] @@ -71,7 +58,7 @@ def get_all_coordinates() -> dict[str, list[TaxelCoord]]: Returns: Dict mapping finger name to list of coordinate dicts """ - mapping = _load_finger_mapping() + mapping = FINGER_MODELS return {finger: _load_model_coordinates(model) for finger, model in mapping.items()} @@ -81,5 +68,5 @@ def get_taxel_counts() -> dict[str, int]: Returns: Dict mapping finger name to number of taxels """ - mapping = _load_finger_mapping() + mapping = FINGER_MODELS return {finger: len(_load_model_coordinates(model)) for finger, model in mapping.items()} diff --git a/orca_core/hardware/sensing/sensor_client.py b/orca_core/hardware/sensor_client.py similarity index 98% rename from orca_core/hardware/sensing/sensor_client.py rename to orca_core/hardware/sensor_client.py index 60c583f0..eb10ca0c 100644 --- a/orca_core/hardware/sensing/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -12,44 +12,38 @@ import time import logging -# Configure logging +from orca_core.hardware.sensing.constants import ( + FINGER_NAMES, + DEFAULT_SENSOR_PORT, + DEFAULT_SENSOR_BAUDRATE, + DEFAULT_FINGER_TO_SENSOR_ID, + PROTOCOL_HEADER_REQUEST, + PROTOCOL_HEADER_RESPONSE, + PROTOCOL_HEADER_AUTO, + PROTOCOL_RESERVED, + FUNC_CODE_READ, + FUNC_CODE_WRITE, + ADDR_HARDWARE_VERSION_START, + ADDR_HARDWARE_VERSION_LENGTH, + ADDR_RESET, + ADDR_CONNECTED_SENSORS_START, + ADDR_CONNECTED_SENSORS_LENGTH, + ADDR_NUM_TAXELS_START, + ADDR_NUM_TAXELS_LENGTH, + ADDR_RESULTING_FORCE_START, + ADDR_RESULTING_FORCE_LENGTH, + ADDR_AUTO_DATA_TYPE, + ADDR_AUTO_ENABLE, +) + logger = logging.getLogger(__name__) -FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] -# Exceptions class NoSensorsAvailableError(Exception): """Raised when no sensors are available for communication.""" pass -# Protocol constants -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 -ADDR_HARDWARE_VERSION_START = 0x0000 -ADDR_HARDWARE_VERSION_LENGTH = 16 - -ADDR_RESET = 0x0022 - -ADDR_CONNECTED_SENSORS_START = 0x0010 -ADDR_CONNECTED_SENSORS_LENGTH = 4 - -ADDR_NUM_TAXELS_START = 0x0030 -ADDR_NUM_TAXELS_LENGTH = 56 - -ADDR_RESULTING_FORCE_START = 0x0500 -ADDR_RESULTING_FORCE_LENGTH = 168 - -ADDR_AUTO_DATA_TYPE = 0x0016 -ADDR_AUTO_ENABLE = 0x0017 - - def calculate_checksum(frame: bytes) -> int: """Calculate checksum for the protocol frame. @@ -108,9 +102,7 @@ class SensorConfiguration: expected_payload_size_taxels: int = 0 # Expected bytes for taxel mode expected_payload_size_combined: int = 0 # Expected bytes for resultant + taxel mode timestamp: float = 0.0 # When this config was captured - finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: { - "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 - }) + finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID)) @property def active_sensors(self) -> list[str]: @@ -137,8 +129,8 @@ class SensorClient: """Client for communicating with ORCA Tactile Sensors""" def __init__(self, - port: str = '/dev/ttyUSB0', - baudrate: int = 921600, + port: str = DEFAULT_SENSOR_PORT, + baudrate: int = DEFAULT_SENSOR_BAUDRATE, finger_to_sensor_id: Optional[dict[str, int]] = None): self.port = port @@ -148,9 +140,7 @@ def __init__(self, # Finger-to-sensor-id mapping (configurable wiring) if finger_to_sensor_id is None: - self._finger_to_sensor_id = { - "thumb": 0, "index": 1, "middle": 2, "ring": 3, "pinky": 4 - } + 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: diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 83afe461..588c9012 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1174,7 +1174,7 @@ def connect(self) -> tuple[bool, str]: if not success: return success, msg - from .hardware.sensing.sensor_client import SensorClient + from .hardware.sensor_client import SensorClient self._sensor_client = SensorClient( port=self.config.sensor_port, diff --git a/scripts/tactile_sensing_ui/tactile_ui.py b/scripts/tactile_sensing_ui/tactile_ui.py index a59efbc3..5375772d 100755 --- a/scripts/tactile_sensing_ui/tactile_ui.py +++ b/scripts/tactile_sensing_ui/tactile_ui.py @@ -7,7 +7,7 @@ import os sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from orca_core.hardware.sensing.sensor_client import SensorClient +from orca_core.hardware.sensor_client import SensorClient from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates from orca_core.utils.utils import read_yaml, update_yaml import argparse diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index 7475cd25..aa433289 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -9,13 +9,13 @@ import pytest -from orca_core.hardware.sensing.sensor_client import ( +from orca_core.hardware.sensor_client import ( SensorClient, SensorConfiguration, calculate_checksum, FINGER_NAMES, ) -from orca_core.hardware.sensing.mock_sensor_client import MockSensorClient +from orca_core.hardware.mock_sensor_client import MockSensorClient from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates EXPECTED_TAXEL_COUNTS = { From c7cd1f68c9878aaac4ef287c49a7377dcdbb5992 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 31 Mar 2026 14:44:46 +0200 Subject: [PATCH 03/20] Major refactor of MockSensorClient to reduce duplication with base class, removed hardware version methods --- orca_core/hardware/mock_sensor_client.py | 640 ++++++----------------- orca_core/hardware/sensing/constants.py | 6 +- orca_core/hardware/sensor_client.py | 432 +++++++-------- scripts/tactile_sensing_ui/tactile_ui.py | 7 - tests/test_tactile_sensor.py | 3 +- 5 files changed, 396 insertions(+), 692 deletions(-) diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 9a79724f..29f02f4b 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -7,575 +7,271 @@ # ============================================================================== """Mock client for simulating tactile sensors in automated tests.""" -from dataclasses import dataclass, field -from typing import Optional -import threading +from __future__ import annotations + +from typing import Callable, Optional import time -import random import logging +from orca_core.hardware.sensor_client import SensorClient, NoSensorsAvailableError from orca_core.hardware.sensing.constants import ( FINGER_NAMES, - DEFAULT_FINGER_TO_SENSOR_ID, DEFAULT_TAXEL_COUNTS, DEFAULT_SENSOR_BAUDRATE, + AUTO_DATA_RESULTANT, + AUTO_DATA_TAXELS, ) logger = logging.getLogger(__name__) +ResultantProvider = Callable[[], dict[str, list[float]]] +TaxelProvider = Callable[[], dict[str, list[list[float]]]] + -class NoSensorsAvailableError(Exception): - """Raised when no sensors are available for communication.""" - pass - - -@dataclass -class AutoStreamStats: - frames_ok: int = 0 - frames_bad_lrc: int = 0 - resyncs: int = 0 - last_error_code: int = 0 - parse_ok: int = 0 - parse_errors: int = 0 - last_eff_len: int = 0 - last_payload_len: int = 0 - consecutive_errors: int = 0 - reconfiguration_count: int = 0 - - -@dataclass -class SensorConfiguration: - """Snapshot of connected sensors and their properties.""" - connected: dict[str, bool] = field(default_factory=dict) - num_taxels: dict[str, int] = field(default_factory=dict) - module_indices: dict[str, int] = field(default_factory=dict) - expected_payload_size_resultant: int = 0 - expected_payload_size_taxels: int = 0 - expected_payload_size_combined: int = 0 - timestamp: float = 0.0 - 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.""" - 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: - """Number of currently connected sensors.""" - 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 MockSensorClient: +class MockSensorClient(SensorClient): """Mock client for simulating tactile sensor communication in tests. - This class provides the same interface as SensorClient but returns - simulated data instead of communicating with real hardware. Useful for: - - Automated testing without hardware - - Development and debugging - - CI/CD pipelines + Subclasses SensorClient, 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. - The simulated data can be controlled via: + Control simulated data via: - set_mock_forces(): Set specific force values to return - set_mock_taxels(): Set specific taxel values to return - set_connected_sensors(): Configure which sensors appear connected - - set_noise_level(): Add random noise to simulated data + - 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 = '', - baudrate: int = DEFAULT_SENSOR_BAUDRATE, - connected_sensors: Optional[list[str]] = None, - finger_to_sensor_id: Optional[dict[str, int]] = None): - """Initialize mock sensor client. - - Args: - port: Serial port (ignored, for API compatibility) - baudrate: Baudrate (ignored, for API compatibility) - connected_sensors: List of sensor names to simulate as connected. - Defaults to ["thumb", "index", "middle"] - finger_to_sensor_id: Finger-to-sensor-id mapping (ignored, for API compatibility) - """ - self.port = port - self.baudrate = baudrate - self._connected = False + def __init__( + self, + port: str = "mock", + baudrate: int = DEFAULT_SENSOR_BAUDRATE, + connected_sensors: Optional[list[str]] = None, + finger_to_sensor_id: Optional[dict[str, int]] = None, + resultant_provider: Optional[ResultantProvider] = None, + taxel_provider: Optional[TaxelProvider] = None, + auto_rate_hz: Optional[float] = None, + ): + super().__init__(port=port, baudrate=baudrate, finger_to_sensor_id=finger_to_sensor_id) - # Configure which sensors appear connected if connected_sensors is None: - connected_sensors = ["thumb", "index", "middle"] - self._simulated_connected = {f: f in connected_sensors for f in FINGER_NAMES} - self._simulated_taxel_counts = { - f: DEFAULT_TAXEL_COUNTS[f] if self._simulated_connected[f] else 0 + connected_sensors = list(FINGER_NAMES) + + self._sim_connected: dict[str, bool] = { + f: f in connected_sensors for f in FINGER_NAMES + } + self._sim_taxel_counts: dict[str, int] = { + f: DEFAULT_TAXEL_COUNTS[f] if f in connected_sensors else 0 for f in FINGER_NAMES } - # Mock data storage self._mock_forces: dict[str, list[float]] = {} self._mock_taxels: dict[str, list[list[float]]] = {} - self._noise_level = 0.0 - self._hardware_version = "MOCK_V1.0.0" - - # Auto-stream state - self._sensor_config: Optional[SensorConfiguration] = None - self._auto_thread: Optional[threading.Thread] = 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._auto_rate_hz = 100 # Simulated update rate - - # Initialize default mock data - self._initialize_mock_data() - - def _initialize_mock_data(self): - """Initialize default mock force and taxel data.""" - for finger in FINGER_NAMES: - if self._simulated_connected[finger]: - self._mock_forces[finger] = [0.0, 0.0, 0.0] - taxel_count = self._simulated_taxel_counts[finger] - self._mock_taxels[finger] = [[0.0, 0.0, 0.0] for _ in range(taxel_count)] + + 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 + ) + + # None = no sleep between frames (ideal for tests). Pass e.g. 1000 + # to throttle to ~1kHz for demos or UI prototyping. + self._auto_rate_hz = auto_rate_hz # ========================================================================= - # Mock Control Methods (for test setup) + # Mock Control Methods # ========================================================================= - def set_connected_sensors(self, sensors: list[str]): + def set_connected_sensors(self, sensors: list[str]) -> None: """Configure which sensors appear as connected. - Args: - sensors: List of finger names to simulate as connected + Only clears mock data for sensors that were removed. """ - self._simulated_connected = {f: f in sensors for f in FINGER_NAMES} - self._simulated_taxel_counts = { - f: DEFAULT_TAXEL_COUNTS[f] if self._simulated_connected[f] else 0 - for f in FINGER_NAMES - } - self._initialize_mock_data() - - # Update configuration if already connected - if self._connected: - self._sensor_config = self._get_configuration() - - def set_mock_forces(self, forces: dict[str, list[float]]): + removed = {f for f, on in self._sim_connected.items() if on and f not in sensors} + self._update_connectivity(sensors) + for f in removed: + self._mock_forces.pop(f, None) + self._mock_taxels.pop(f, None) + + def simulate_dropout(self, dropped: list[str]) -> None: + """Simulate one or more sensors dropping out.""" + remaining = [f for f in self._sim_connected if self._sim_connected[f] and f not in dropped] + self.set_connected_sensors(remaining) + + def set_mock_forces(self, forces: dict[str, list[float]]) -> None: """Set the force values to return for each sensor. - Args: - forces: Dict mapping finger names to [fx, fy, fz] values + Replaces all previously set mock forces. + + Raises: + ValueError: If any finger name is not valid or force vector is wrong length """ - for finger, force in forces.items(): - if finger in FINGER_NAMES and len(force) == 3: - self._mock_forces[finger] = list(force) + 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: dict[str, list[list[float]]]): + def set_mock_taxels(self, taxels: dict[str, list[list[float]]]) -> None: """Set the taxel values to return for each sensor. - Args: - taxels: Dict mapping finger names to list of [fx, fy, fz] per taxel - """ - for finger, data in taxels.items(): - if finger in FINGER_NAMES: - self._mock_taxels[finger] = [list(t) for t in data] - - def set_noise_level(self, level: float): - """Set random noise level to add to returned data. + Replaces all previously set mock taxels. - Args: - level: Standard deviation of Gaussian noise to add (in Newtons) + Raises: + ValueError: If any finger name is not valid or any taxel vector is wrong length """ - self._noise_level = level + 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_hardware_version(self, version: str): - """Set the hardware version string to return. + def set_resultant_provider(self, provider: ResultantProvider) -> None: + """Inject a deterministic resultant provider (called per read).""" + self._resultant_provider = provider - Args: - version: Version string to return from read_hardware_version() - """ - self._hardware_version = version - - def set_auto_rate(self, rate_hz: float): - """Set the simulated auto-stream update rate. - - Args: - rate_hz: Updates per second for auto-stream simulation - """ - self._auto_rate_hz = rate_hz + def set_taxel_provider(self, provider: TaxelProvider) -> None: + """Inject a deterministic taxel provider (called per read).""" + self._taxel_provider = provider # ========================================================================= - # Connection Methods + # Connection (no-op hardware I/O) # ========================================================================= - @property - def is_connected(self) -> bool: - """Check if client is connected.""" - return self._connected - - def connect(self): - """Simulate connecting to sensor device.""" + def connect(self) -> None: if self.is_connected: return - self._connected = True - logger.info(f"[MOCK] Connected to sensor at {self.port}") - - # Build initial configuration self._sensor_config = self._get_configuration() - logger.info(f"[MOCK] Initial configuration: {self._sensor_config}") + logger.info(f"[MOCK] Connected, config: {self._sensor_config}") - def disconnect(self): - """Simulate disconnecting from sensor device.""" + 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 from sensor") + logger.info("[MOCK] Disconnected") # ========================================================================= - # Sensor Information Methods + # Sensor Information (return simulated state) # ========================================================================= - def read_hardware_version(self) -> str: - """Return mock hardware version.""" - if not self.is_connected: - raise OSError("Must call connect() first.") - return self._hardware_version - def read_connected_sensors(self) -> dict[str, bool]: - """Return simulated connected sensor status.""" if not self.is_connected: raise OSError("Must call connect() first.") - return dict(self._simulated_connected) + return dict(self._sim_connected) def read_num_taxels(self) -> dict[str, int]: - """Return simulated taxel counts.""" if not self.is_connected: raise OSError("Must call connect() first.") - return dict(self._simulated_taxel_counts) + return dict(self._sim_taxel_counts) def read_auto_data_type(self) -> dict: - """Return simulated auto data type configuration.""" if not self.is_connected: raise OSError("Must call connect() first.") - - val = (0x01 if self._auto_mode_resultant else 0) | (0x02 if self._auto_mode_taxels else 0) + 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}", "resulting_force": self._auto_mode_resultant, "individual_taxels_force": self._auto_mode_taxels, } - def get_sensor_configuration(self) -> Optional[SensorConfiguration]: - """Get the current sensor configuration snapshot.""" - return self._sensor_config - - def _get_configuration(self) -> SensorConfiguration: - """Build configuration from current simulated state.""" - connected = dict(self._simulated_connected) - num_taxels = dict(self._simulated_taxel_counts) - - module_indices = {} - for i, finger in enumerate(FINGER_NAMES): - if connected.get(finger, False): - module_indices[finger] = i * 4 + 2 - - num_active = sum(1 for c in connected.values() if c) - expected_resultant = num_active * 6 - expected_taxels = sum( - num_taxels.get(finger, 0) * 3 - for finger, is_connected in connected.items() - if is_connected - ) - expected_combined = expected_resultant + expected_taxels - - return SensorConfiguration( - connected=connected, - num_taxels=num_taxels, - module_indices=module_indices, - expected_payload_size_resultant=expected_resultant, - expected_payload_size_taxels=expected_taxels, - expected_payload_size_combined=expected_combined, - timestamp=time.time() - ) - # ========================================================================= - # Force Reading Methods + # Hardware I/O overrides (no-ops) # ========================================================================= - def _add_noise(self, value: float) -> float: - """Add Gaussian noise to a value.""" - if self._noise_level > 0: - return value + random.gauss(0, self._noise_level) - return value - - def _get_mock_forces(self) -> dict[str, list[float]]: - """Get mock forces with optional noise.""" - result = {} - for finger in FINGER_NAMES: - if self._simulated_connected.get(finger, False): - base = self._mock_forces.get(finger, [0.0, 0.0, 0.0]) - result[finger] = [ - round(self._add_noise(base[0]), 1), - round(self._add_noise(base[1]), 1), - round(self._add_noise(base[2]), 1), - ] - return result - - def _get_mock_taxels(self) -> dict[str, list[list[float]]]: - """Get mock taxels with optional noise.""" - result = {} - for finger in FINGER_NAMES: - if self._simulated_connected.get(finger, False): - base_taxels = self._mock_taxels.get(finger, []) - result[finger] = [ - [ - round(self._add_noise(t[0]), 2), - round(self._add_noise(t[1]), 2), - round(self._add_noise(t[2]), 2), - ] - for t in base_taxels - ] - return result - - def read_resulting_force(self) -> dict[str, list[float]]: - """Return simulated resultant forces.""" - if not self.is_connected: - raise OSError("Must call connect() first.") - return self._get_mock_forces() - - # ========================================================================= - # Auto-Stream Control Methods - # ========================================================================= - - def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: - """Configure data types for auto stream.""" - if not self.is_connected: - raise OSError("Must call connect() first.") - self._auto_mode_resultant = resultant - self._auto_mode_taxels = taxels - - def enable_auto_data_transmission(self) -> None: - """Enable auto data transmission (no-op in mock).""" - if not self.is_connected: - raise OSError("Must call connect() first.") - - def disable_auto_data_transmission(self) -> None: - """Disable auto data transmission (no-op in mock).""" - if not self.is_connected: - raise OSError("Must call connect() first.") + def _write_register(self, address: int, data: bytes, response_timeout_s: float = 0.5) -> None: + pass def reboot(self) -> None: - """Simulate sensor reboot.""" if not self.is_connected: raise OSError("Must call connect() first.") logger.info("[MOCK] Sensor reboot simulated") # ========================================================================= - # Auto-Stream Methods + # Force Reading # ========================================================================= - def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): - """Background thread that simulates auto-stream data generation.""" - interval = 1.0 / self._auto_rate_hz - - while self._auto_running.is_set(): - try: - parsed_resultant = None - parsed_taxels = None - - if parse_resultant: - parsed_resultant = self._get_mock_forces() - if parse_taxels: - parsed_taxels = self._get_mock_taxels() - - with self._auto_lock: - if parse_resultant: - self._auto_latest = parsed_resultant - if parse_taxels: - self._auto_latest_taxels = parsed_taxels - self._auto_latest_ts = time.time() - self._auto_stats.frames_ok += 1 - self._auto_stats.parse_ok += 1 - - time.sleep(interval) - - except Exception as e: - logger.error(f"[MOCK] Error in auto reader: {e}") - with self._auto_lock: - self._auto_stats.parse_errors += 1 - time.sleep(interval) - - logger.info("[MOCK] Auto reader loop exited") - - def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1): - """Start simulated auto-stream mode. - - Args: - resultant: Include resultant force data - taxels: Include taxel data - min_sensors: Minimum sensors required - - Raises: - OSError: If not connected - NoSensorsAvailableError: If fewer than min_sensors available - ValueError: If neither resultant nor taxels enabled - """ - 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") + def _read_raw_resultant(self) -> dict[str, list[float]]: + return self._resultant_provider() - self.stop_auto_stream() - - self._auto_mode_resultant = resultant - self._auto_mode_taxels = taxels - - self._sensor_config = self._get_configuration() - - if self._sensor_config.num_active_sensors < min_sensors: - raise NoSensorsAvailableError( - f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " - f"need at least {min_sensors}" - ) - - mode_str = [] - if resultant: - mode_str.append("resultant") - if taxels: - mode_str.append("taxels") - logger.info( - f"[MOCK] Starting auto-stream with {self._sensor_config}, " - f"mode={'+'.join(mode_str)}" - ) - - self._auto_running.set() - self._auto_thread = threading.Thread( - target=self._auto_reader_loop, - args=(resultant, taxels), - daemon=True - ) - self._auto_thread.start() + # ========================================================================= + # Frame Acquisition (overrides base class serial reader) + # ========================================================================= - def stop_auto_stream(self): - """Stop simulated auto-stream mode.""" - self._auto_running.clear() + def _acquire_frame( + self, + parse_resultant: bool, + parse_taxels: bool, + min_sensors: int, + ) -> tuple[dict | None, dict | None]: + """Acquire simulated frame data from mock providers.""" + if self._sensor_config is None or self._sensor_config.num_active_sensors < min_sensors: + raise NoSensorsAvailableError("Insufficient sensors for auto-stream") - if self._auto_thread is not None: - self._auto_thread.join(timeout=1.0) - self._auto_thread = None + parsed_resultant = self._resultant_provider() if parse_resultant else None + parsed_taxels = self._taxel_provider() if parse_taxels else None - with self._auto_lock: - self._auto_latest = None - self._auto_latest_taxels = None - self._auto_latest_ts = None + # Rate limiting (None = no sleep, ideal for tests) + if self._auto_rate_hz: + time.sleep(1.0 / self._auto_rate_hz) - def get_auto_latest(self): - """Get latest simulated resultant force data.""" - with self._auto_lock: - return self._auto_latest, self._auto_latest_ts + return parsed_resultant, parsed_taxels - def get_auto_latest_taxels(self): - """Get latest simulated taxel data.""" - with self._auto_lock: - return self._auto_latest_taxels, self._auto_latest_ts + # ========================================================================= + # Internal Helpers + # ========================================================================= - def get_auto_latest_all(self): - """Get all latest simulated data.""" - with self._auto_lock: - return self._auto_latest, self._auto_latest_taxels, self._auto_latest_ts + def _update_connectivity(self, sensors: list[str]) -> None: + """Update simulated connectivity and reconfigure if connected.""" + self._sim_connected = {f: f in sensors for f in FINGER_NAMES} + self._sim_taxel_counts = { + f: DEFAULT_TAXEL_COUNTS[f] if self._sim_connected[f] else 0 + for f in FINGER_NAMES + } + if self._connected: + self._sensor_config = self._get_configuration() - def get_auto_stats(self): - """Get auto-stream statistics.""" - with self._auto_lock: - return self._auto_stats + def _default_resultant_provider(self) -> dict[str, list[float]]: + 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) + } - # ========================================================================= - # Context Manager Support - # ========================================================================= + def _default_taxel_provider(self) -> dict[str, list[list[float]]]: + 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 - def __enter__(self): - """Enable use as context manager.""" - if not self.is_connected: - self.connect() - return self - - def __exit__(self, *args): - """Enable use as context manager.""" - self.disconnect() - - def __del__(self): - """Cleanup on destruction.""" - try: - self.disconnect() - except Exception: - pass - - -if __name__ == "__main__": - # Simple test of mock client - logging.basicConfig(level=logging.INFO) - - print("=== Mock Sensor Client Test ===\n") - - with MockSensorClient(connected_sensors=["thumb", "index", "middle"]) as client: - print(f"Hardware version: {client.read_hardware_version()}") - print(f"Connected sensors: {client.read_connected_sensors()}") - print(f"Taxel counts: {client.read_num_taxels()}") - print(f"Configuration: {client.get_sensor_configuration()}") - - # Test request-response mode - print("\n--- Request-Response Mode ---") - forces = client.read_resulting_force() - print(f"Forces: {forces}") - - # Set specific mock values - client.set_mock_forces({ - "thumb": [1.0, 0.5, 2.0], - "index": [0.0, 0.0, 1.5], - "middle": [-0.5, 0.2, 0.8], - }) - forces = client.read_resulting_force() - print(f"Forces (with mock values): {forces}") - - # Test with noise - client.set_noise_level(0.1) - forces = client.read_resulting_force() - print(f"Forces (with noise): {forces}") - client.set_noise_level(0.0) - - # Test auto-stream mode - print("\n--- Auto-Stream Mode (resultant only) ---") - client.start_auto_stream(resultant=True, taxels=False) - time.sleep(0.1) - for _ in range(5): - forces, ts = client.get_auto_latest() - if forces: - print(f"[{ts:.3f}] {forces}") - time.sleep(0.05) - client.stop_auto_stream() - - # Test combined mode - print("\n--- Auto-Stream Mode (combined) ---") - client.start_auto_stream(resultant=True, taxels=True) - time.sleep(0.1) - forces, taxels, ts = client.get_auto_latest_all() - if forces: - print(f"Forces: {forces}") - print(f"Taxel counts: {({f: len(t) for f, t in taxels.items()}) if taxels else 'None'}") - client.stop_auto_stream() - - # Show stats - stats = client.get_auto_stats() - print(f"\nStats: frames_ok={stats.frames_ok}, parse_ok={stats.parse_ok}") - - print("\n=== Test Complete ===") + @staticmethod + def _validate_finger_vectors( + data: dict[str, list[float]], 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/constants.py b/orca_core/hardware/sensing/constants.py index c79376cd..9892f168 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -33,8 +33,6 @@ FUNC_CODE_WRITE = 0x10 # Register addresses -ADDR_HARDWARE_VERSION_START = 0x0000 -ADDR_HARDWARE_VERSION_LENGTH = 16 ADDR_RESET = 0x0022 ADDR_CONNECTED_SENSORS_START = 0x0010 ADDR_CONNECTED_SENSORS_LENGTH = 4 @@ -44,3 +42,7 @@ ADDR_RESULTING_FORCE_LENGTH = 168 ADDR_AUTO_DATA_TYPE = 0x0016 ADDR_AUTO_ENABLE = 0x0017 + +# Auto data type bitmasks +AUTO_DATA_RESULTANT = 0x01 +AUTO_DATA_TAXELS = 0x02 diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index eb10ca0c..e46ed5f0 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -23,8 +23,6 @@ PROTOCOL_RESERVED, FUNC_CODE_READ, FUNC_CODE_WRITE, - ADDR_HARDWARE_VERSION_START, - ADDR_HARDWARE_VERSION_LENGTH, ADDR_RESET, ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH, @@ -34,6 +32,8 @@ ADDR_RESULTING_FORCE_LENGTH, ADDR_AUTO_DATA_TYPE, ADDR_AUTO_ENABLE, + AUTO_DATA_RESULTANT, + AUTO_DATA_TAXELS, ) logger = logging.getLogger(__name__) @@ -44,6 +44,18 @@ 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 + + def calculate_checksum(frame: bytes) -> int: """Calculate checksum for the protocol frame. @@ -170,6 +182,7 @@ def __init__(self, self._auto_stats = AutoStreamStats() self._auto_mode_resultant = True # Whether to parse resultant forces self._auto_mode_taxels = False # Whether to parse taxels + self._last_frame_debug_print: float = 0.0 # Per-taxel zeroing offsets self._taxel_offsets: Optional[dict] = None # {finger: [[fx, fy, fz], ...], ...} @@ -475,23 +488,6 @@ def read_connected_sensors(self) -> dict[str, bool]: ] return {self._sensor_id_to_finger[i]: status[i] for i in range(5)} - def read_hardware_version(self) -> str: - """Read the hardware version. - - Returns: - Hardware version string - - Raises: - OSError: If not connected to sensor - """ - if not self.is_connected: - raise OSError("Must call connect() first.") - - version_bytes = self._read_register(ADDR_HARDWARE_VERSION_START, ADDR_HARDWARE_VERSION_LENGTH) - version_string = version_bytes.decode('ascii', errors='ignore').rstrip('\x00').rstrip() - - return version_string - def read_num_taxels(self) -> dict[str, int]: """Read the number of taxels for each fingertip sensor. @@ -529,29 +525,20 @@ def read_auto_data_type(self) -> dict: byte_val = data[0] return { "raw": f"{byte_val:08b}", - "resulting_force": bool(byte_val & 0x01), - "individual_taxels_force": bool(byte_val & 0x02), + "resulting_force": bool(byte_val & AUTO_DATA_RESULTANT), + "individual_taxels_force": bool(byte_val & AUTO_DATA_TAXELS), } - def read_resulting_force(self) -> dict[str, list[float]]: - """Read resulting force from all connected fingertip sensors. - - Uses dynamic parsing based on current sensor configuration. Returns data - only for sensors that are currently connected (sparse dict format). + def _read_raw_resultant(self) -> dict[str, list[float]]: + """Read raw resultant forces from hardware (no offset application). - Note: This implementation assumes we only have fingertip (distal phalanx) sensors - for each finger, not proximal/middle phalanx or palm sensors. + Subclasses (e.g. MockSensorClient) override this to return simulated + data. The public read_resulting_force() method calls this, then applies + zeroing offsets. Returns: Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons - Only includes sensors that are currently connected - - Raises: - OSError: If not connected to sensor """ - if not self.is_connected: - raise OSError("Must call connect() first.") - # Ensure we have current configuration if self._sensor_config is None: try: @@ -562,9 +549,26 @@ def read_resulting_force(self) -> dict[str, list[float]]: data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) return self._parse_resultant_force_block(data) - # Use dynamic parsing based on configuration data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) - result = self._parse_resultant_force_dynamic(data, self._sensor_config) + return self._parse_resultant_force_dynamic(data, self._sensor_config) + + def read_resulting_force(self) -> dict[str, list[float]]: + """Read resulting force from all connected fingertip sensors. + + Calls _read_raw_resultant() for data, then applies zeroing offsets. + Subclasses should override _read_raw_resultant(), not this method. + + Returns: + Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons + Only includes sensors that are currently connected + + Raises: + OSError: If not connected to sensor + """ + 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 @@ -896,7 +900,7 @@ def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> No if not self.is_connected: raise OSError("Must call connect() first.") - val = (0x01 if resultant else 0) | (0x02 if taxels else 0) + val = (AUTO_DATA_RESULTANT if resultant else 0) | (AUTO_DATA_TAXELS if taxels else 0) self._write_register(ADDR_AUTO_DATA_TYPE, bytes([val])) @@ -1129,6 +1133,20 @@ def _apply_resultant_offsets(self, forces: dict) -> None: fvec[1] = round(fvec[1] - off[1], 1) fvec[2] = round(max(0, fvec[2] - off[2]), 1) + def _apply_stream_offsets( + self, + parsed_resultant: Optional[dict], + parsed_taxels: Optional[dict], + ) -> None: + """Apply zeroing offsets to parsed auto-stream data in-place. + + Called by _auto_reader_loop implementations after parsing raw data. + """ + 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 from serial connection, blocking until complete. @@ -1230,208 +1248,197 @@ def _get_expected_payload_size(self, config: SensorConfiguration) -> int: return config.expected_payload_size_taxels return 0 - def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_sensors: int): - """Background thread that continuously reads and parses auto-stream frames. + def _acquire_frame( + self, + parse_resultant: bool, + parse_taxels: bool, + min_sensors: int, + ) -> tuple[Optional[dict], Optional[dict]]: + """Acquire and return the next parsed (resultant, taxels) frame. + + Reads one auto-stream frame from serial, validates LRC, handles payload + size mismatches with reconfiguration, and parses the data. + + Subclasses (e.g. MockSensorClient) override this to provide data from + other sources while inheriting the loop's stats, offset, and lifecycle logic. + + Returns: + Tuple of (parsed_resultant, parsed_taxels). Either may be None + if not requested. + + Raises: + FrameError: Recoverable frame-level error (bad LRC, parse failure) + NoSensorsAvailableError: No sensors available after reconfiguration + IOError: Serial communication failure or auto stream stopped + """ + # Find and consume AA 56 header + self._resync_to_auto_header() + + # Read frame metadata + reserved = self._read_exact(1) + eff_len = int.from_bytes(self._read_exact(2), "little") + + # Read payload and checksum + payload = self._read_exact(eff_len) + lrc = self._read_exact(1)[0] + + # Debug print (throttled to once per second) + now = time.time() + if now - self._last_frame_debug_print > 1.0: + config_str = str(self._sensor_config) if self._sensor_config else "no config" + logger.debug(f"[auto] eff_len={eff_len}, config={config_str}") + self._last_frame_debug_print = now + + # Validate frame integrity + frame_wo_lrc = bytes([0xAA, 0x56]) + reserved + int_to_little_endian(eff_len, 2) + payload + if not self._check_lrc(frame_wo_lrc, lrc): + raise FrameError("LRC mismatch", bad_lrc=True) + + # Split error code and valid data + err_code = payload[0] + valid = payload[1:] + + # Update serial-specific stats + with self._auto_lock: + self._auto_stats.last_error_code = err_code + self._auto_stats.last_eff_len = eff_len + self._auto_stats.last_payload_len = len(valid) + + # Check for payload size mismatch (indicates config change) + if self._sensor_config: + expected_size = self._get_expected_payload_size(self._sensor_config) + if len(valid) != expected_size and expected_size > 0: + logger.warning( + f"Payload size mismatch: expected {expected_size}, got {len(valid)}. " + "Triggering reconfiguration..." + ) + try: + if self._reconfigure(force=False): + logger.info("Reconfiguration successful, continuing stream") + except NoSensorsAvailableError: + raise + except Exception as e: + logger.error(f"Reconfiguration failed: {e}") + raise FrameError("Payload size mismatch, skipping frame") + + # Parse payload based on mode + if not self._sensor_config: + raise FrameError("No sensor configuration available") - Auto-stream frame format (when enabled via 0x0017 = 1): - - Header: AA 56 (2 bytes) - - Reserved: 0x00 (1 byte) - - Effective length: eff_len (2 bytes, little-endian) - - Error code: (1 byte) - part of eff_len - - Valid data: (eff_len-1 bytes) - sensor force data - - LRC: (1 byte) - checksum + expected_size = self._get_expected_payload_size(self._sensor_config) + if len(valid) != expected_size or expected_size == 0: + raise FrameError( + f"Unexpected payload: {len(valid)} bytes, expected {expected_size}" + ) - The sensor continuously sends these frames at ~1kHz when auto mode is enabled. - This thread parses them and updates _auto_latest for the user to read via - get_auto_latest(). + if parse_resultant and parse_taxels: + parsed_resultant, parsed_taxels = self._parse_combined_compact( + valid, self._sensor_config + ) + elif parse_resultant: + parsed_resultant = self._parse_auto_stream_compact(valid, self._sensor_config) + parsed_taxels = None + elif parse_taxels: + parsed_resultant = None + parsed_taxels = self._parse_taxels_compact(valid, self._sensor_config) + else: + parsed_resultant = None + parsed_taxels = None - Supports three modes: - - Resultant only: 6 bytes per sensor (fx, fy, fz) - - Taxels only: 2 bytes per taxel for each sensor - - Combined: Resultant forces followed by taxels + return parsed_resultant, parsed_taxels - Features error-triggered reconfiguration: if consecutive errors exceed threshold, - attempts to reconfigure to adapt to changed sensor configuration. + def _handle_error_threshold_reconfiguration(self, min_sensors: int) -> None: + """Attempt reconfiguration when consecutive errors exceed threshold. + + Called by the auto-reader loop. Stops the stream if no sensors remain + or if the minimum sensor requirement is no longer met. + """ + logger.warning( + f"Consecutive errors ({self._auto_stats.consecutive_errors}) exceeded threshold. " + "Attempting reconfiguration..." + ) + try: + if self._reconfigure(force=False): + logger.info("Reconfiguration successful") + if self._sensor_config.num_active_sensors < min_sensors: + logger.error( + f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " + f"need {min_sensors}. Stopping stream." + ) + self._auto_running.clear() + except NoSensorsAvailableError: + logger.error("No sensors available, stopping stream") + self._auto_running.clear() + except Exception as e: + logger.error(f"Reconfiguration failed: {e}") + with self._auto_lock: + self._auto_stats.consecutive_errors = 0 + + def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_sensors: int): + """Background thread that continuously reads and parses auto-stream frames. + + Calls _acquire_frame() to get parsed data, then applies offsets and updates + stats. Subclasses override _acquire_frame() to change the data source while + inheriting the shared loop logic. Args: parse_resultant: Whether to parse resultant force data parse_taxels: Whether to parse individual taxel data min_sensors: Minimum number of sensors required to continue streaming """ - last_print = 0.0 - ERROR_THRESHOLD = 5 # Trigger reconfiguration after 5 consecutive errors + ERROR_THRESHOLD = 5 while self._auto_running.is_set(): try: - # ===== Step 1: Find and consume AA 56 header ===== - self._resync_to_auto_header() - - # ===== Step 2: Read frame metadata ===== - reserved = self._read_exact(1) # Typically 0x00 - eff_len = int.from_bytes(self._read_exact(2), "little") - - # ===== Step 3: Read payload and checksum ===== - # Payload includes: error_code(1) + valid_data(eff_len-1) - payload = self._read_exact(eff_len) - lrc = self._read_exact(1)[0] - - # Debug print (throttled to once per second) - now = time.time() - if now - last_print > 1.0: - config_str = str(self._sensor_config) if self._sensor_config else "no config" - logger.debug(f"[auto] eff_len={eff_len}, config={config_str}") - last_print = now - - # ===== Step 4: Validate frame integrity ===== - frame_wo_lrc = bytes([0xAA, 0x56]) + reserved + int_to_little_endian(eff_len, 2) + payload - if not self._check_lrc(frame_wo_lrc, lrc): - with self._auto_lock: - self._auto_stats.frames_bad_lrc += 1 - self._auto_stats.consecutive_errors += 1 - continue # Skip corrupted frame, resync - - # ===== Step 5: Split error code and valid data ===== - err_code = payload[0] - valid = payload[1:] # The actual sensor data - - parsed_resultant = None - parsed_taxels = None - parse_success = False - - # ===== Step 6: Check for payload size mismatch (indicates config change) ===== - if self._sensor_config: - expected_size = self._get_expected_payload_size(self._sensor_config) - if len(valid) != expected_size and expected_size > 0: - # Payload size changed! Trigger immediate reconfiguration - logger.warning( - f"Payload size mismatch: expected {expected_size}, got {len(valid)}. " - "Triggering reconfiguration..." - ) - try: - if self._reconfigure(force=False): - logger.info("Reconfiguration successful, continuing stream") - continue # Skip this frame, let next iteration use new config - except NoSensorsAvailableError: - logger.error("No sensors available after reconfiguration, stopping stream") - self._auto_running.clear() - break - except Exception as e: - logger.error(f"Reconfiguration failed: {e}") - # Continue with old config - - # ===== Step 7: Parse payload based on mode ===== - if self._sensor_config: - try: - expected_size = self._get_expected_payload_size(self._sensor_config) - - if len(valid) == expected_size and expected_size > 0: - # Combined mode: resultant + taxels - if parse_resultant and parse_taxels: - parsed_resultant, parsed_taxels = self._parse_combined_compact( - valid, self._sensor_config - ) - parse_success = True - - # Resultant only mode - elif parse_resultant: - parsed_resultant = self._parse_auto_stream_compact( - valid, self._sensor_config - ) - parse_success = True - - # Taxels only mode - elif parse_taxels: - parsed_taxels = self._parse_taxels_compact( - valid, self._sensor_config - ) - parse_success = True - - else: - # Unexpected size - logger.warning( - f"Unexpected payload: {len(valid)} bytes, expected {expected_size}" - ) - parse_success = False - - except Exception as e: - logger.error(f"Parse error: {e}", exc_info=True) - parse_success = False - - # ===== Step 8: Apply zeroing offsets ===== - if parse_success: - 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) - - # ===== Step 9: Publish latest data and update stats ===== - with self._auto_lock: - if parse_success: - if parse_resultant: - self._auto_latest = parsed_resultant - if parse_taxels: - self._auto_latest_taxels = parsed_taxels - self._auto_latest_ts = time.time() - self._auto_stats.consecutive_errors = 0 # Reset on success + parsed_resultant, parsed_taxels = self._acquire_frame( + parse_resultant, parse_taxels, min_sensors + ) + + 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._auto_stats.last_error_code = err_code - self._auto_stats.last_eff_len = eff_len - self._auto_stats.last_payload_len = len(valid) + self._auto_stats.parse_ok += 1 + self._auto_stats.consecutive_errors = 0 - if parse_success: - self._auto_stats.parse_ok += 1 + except FrameError as e: + with self._auto_lock: + if e.bad_lrc: + self._auto_stats.frames_bad_lrc += 1 else: + self._auto_stats.frames_ok += 1 self._auto_stats.parse_errors += 1 - self._auto_stats.consecutive_errors += 1 + self._auto_stats.consecutive_errors += 1 - # ===== Step 10: Check if we should trigger reconfiguration ===== - if self._auto_stats.consecutive_errors >= ERROR_THRESHOLD: - logger.warning( - f"Consecutive errors ({self._auto_stats.consecutive_errors}) exceeded threshold. " - "Attempting reconfiguration..." - ) - try: - if self._reconfigure(force=False): - logger.info("Reconfiguration successful") - - # Check if we still meet minimum sensor requirement - if self._sensor_config.num_active_sensors < min_sensors: - logger.error( - f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " - f"need {min_sensors}. Stopping stream." - ) - self._auto_running.clear() - break - except NoSensorsAvailableError: - logger.error("No sensors available, stopping stream") - self._auto_running.clear() - break - except Exception as e: - logger.error(f"Reconfiguration failed: {e}") - # Reset error counter to avoid infinite reconfiguration attempts - with self._auto_lock: - self._auto_stats.consecutive_errors = 0 + except NoSensorsAvailableError: + logger.error("No sensors available, stopping stream") + self._auto_running.clear() + break except IOError as e: if "Auto stream stopped" in str(e): - # Normal shutdown, exit gracefully logger.info("Auto stream stopped") break - # Other IO errors logger.warning(f"IO error in auto reader: {e}") with self._auto_lock: self._auto_stats.resyncs += 1 self._auto_stats.consecutive_errors += 1 - time.sleep(0.01) # Brief pause before retry + time.sleep(0.01) except Exception as e: - # Unexpected errors logger.error(f"Unexpected error in auto reader: {e}", exc_info=True) with self._auto_lock: self._auto_stats.resyncs += 1 self._auto_stats.consecutive_errors += 1 - time.sleep(0.01) # Brief pause before retry + time.sleep(0.01) + + if self._auto_stats.consecutive_errors >= ERROR_THRESHOLD: + self._handle_error_threshold_reconfiguration(min_sensors) logger.info("Auto reader loop exited") @@ -1516,7 +1523,8 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se self.set_auto_data_type(resultant=resultant, taxels=taxels) # Clear any stale data from serial buffer before starting - self._serial_connection.reset_input_buffer() + if self._serial_connection is not None: + self._serial_connection.reset_input_buffer() # Enable auto transmission mode (sensor starts broadcasting) self.enable_auto_data_transmission() @@ -1564,6 +1572,13 @@ def stop_auto_stream(self): 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() if __name__ == "__main__": @@ -1575,7 +1590,6 @@ def stop_auto_stream(self): print(sensor_client._sensor_config) exit() try: - print("version:", sensor_client.read_hardware_version()) print("connected sensors:", sensor_client.read_connected_sensors()) print("num taxels:", sensor_client.read_num_taxels()) print("config:", sensor_client.get_sensor_configuration()) @@ -1623,4 +1637,4 @@ def stop_auto_stream(self): sensor_client.stop_auto_stream() except Exception: pass - sensor_client.disconnect() \ No newline at end of file + sensor_client.disconnect() diff --git a/scripts/tactile_sensing_ui/tactile_ui.py b/scripts/tactile_sensing_ui/tactile_ui.py index 5375772d..97738620 100755 --- a/scripts/tactile_sensing_ui/tactile_ui.py +++ b/scripts/tactile_sensing_ui/tactile_ui.py @@ -234,11 +234,6 @@ def status(): out = {'connected': True, 'mode': current_mode} errors = [] - try: - out['hardware_version'] = client.read_hardware_version() - except Exception as e: - errors.append(f"read_hardware_version: {e}") - try: out['sensors'] = client.read_connected_sensors() except Exception as e: @@ -297,14 +292,12 @@ def refresh(): return jsonify({'connected': False}) connected = client.read_connected_sensors() - version = client.read_hardware_version() taxels = client.read_num_taxels() auto_data = client.read_auto_data_type() forces = client.read_resulting_force() return jsonify({ 'connected': True, - 'hardware_version': version, 'sensors': connected, 'taxels': taxels, 'auto_data_type': auto_data, diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index aa433289..84caaab9 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -86,9 +86,8 @@ def test_shape_all_fingers(self): assert len(result[finger]) == 3 assert all(isinstance(v, float) for v in result[finger]) - def test_values_match_no_noise(self): + def test_values_match(self): mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.set_noise_level(0.0) mock.connect() mock.set_mock_forces({"thumb": [1.5, -2.0, 3.0], "index": [0.0, 0.0, 0.0]}) mock.start_auto_stream(resultant=True, taxels=False) From 16d68cdd342fae6f97954200c56ccee444279a54 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Fri, 3 Apr 2026 11:42:26 +0200 Subject: [PATCH 04/20] Extract protocol codec layer and harden contracts Introduce protocol.py as a pure-function codec for the sensor wire format (frame building, parsing, decoding). Tighten validation (header checks, meta size guards, strict KeyError on missing taxels), move wire-format constants to constants.py, and reduce protocol knowledge leaking into sensor_client.py. --- .gitignore | 3 + orca_core/hardware/mock_sensor_client.py | 29 +- orca_core/hardware/sensing/__init__.py | 0 orca_core/hardware/sensing/constants.py | 70 ++- orca_core/hardware/sensing/protocol.py | 642 +++++++++++++++++++++++ orca_core/hardware/sensor_client.py | 553 ++++--------------- scripts/tactile_sensing_ui/tactile_ui.py | 4 +- tests/test_protocol.py | 527 +++++++++++++++++++ tests/test_tactile_sensor.py | 431 ++++++++------- tests/test_taxel_coordinates.py | 22 + 10 files changed, 1575 insertions(+), 706 deletions(-) create mode 100644 orca_core/hardware/sensing/__init__.py create mode 100644 orca_core/hardware/sensing/protocol.py create mode 100644 tests/test_protocol.py create mode 100644 tests/test_taxel_coordinates.py diff --git a/.gitignore b/.gitignore index e2ca6a0d..02554b94 100644 --- a/.gitignore +++ b/.gitignore @@ -133,3 +133,6 @@ calibration.yaml # not storing the lockfile (as of now) uv.lock + +# Vendor reference documents +docs/references/ diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 29f02f4b..23e6bbe4 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -21,11 +21,16 @@ AUTO_DATA_RESULTANT, AUTO_DATA_TAXELS, ) +from orca_core.hardware.sensing.protocol import ( + ForceVector, + ResultantForces, + TaxelForces, +) logger = logging.getLogger(__name__) -ResultantProvider = Callable[[], dict[str, list[float]]] -TaxelProvider = Callable[[], dict[str, list[list[float]]]] +ResultantProvider = Callable[[], ResultantForces] +TaxelProvider = Callable[[], TaxelForces] class MockSensorClient(SensorClient): @@ -68,8 +73,8 @@ def __init__( for f in FINGER_NAMES } - self._mock_forces: dict[str, list[float]] = {} - self._mock_taxels: dict[str, list[list[float]]] = {} + 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 @@ -102,7 +107,7 @@ def simulate_dropout(self, dropped: list[str]) -> None: remaining = [f for f in self._sim_connected if self._sim_connected[f] and f not in dropped] self.set_connected_sensors(remaining) - def set_mock_forces(self, forces: dict[str, list[float]]) -> None: + def set_mock_forces(self, forces: ResultantForces) -> None: """Set the force values to return for each sensor. Replaces all previously set mock forces. @@ -113,7 +118,7 @@ def set_mock_forces(self, forces: dict[str, list[float]]) -> None: 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: dict[str, list[list[float]]]) -> None: + def set_mock_taxels(self, taxels: TaxelForces) -> None: """Set the taxel values to return for each sensor. Replaces all previously set mock taxels. @@ -177,8 +182,8 @@ def read_auto_data_type(self) -> dict: ) return { "raw": f"{val:08b}", - "resulting_force": self._auto_mode_resultant, - "individual_taxels_force": self._auto_mode_taxels, + "resultant": self._auto_mode_resultant, + "taxels": self._auto_mode_taxels, } # ========================================================================= @@ -197,7 +202,7 @@ def reboot(self) -> None: # Force Reading # ========================================================================= - def _read_raw_resultant(self) -> dict[str, list[float]]: + def _read_raw_resultant(self) -> ResultantForces: return self._resultant_provider() # ========================================================================= @@ -237,7 +242,7 @@ def _update_connectivity(self, sensors: list[str]) -> None: if self._connected: self._sensor_config = self._get_configuration() - def _default_resultant_provider(self) -> dict[str, list[float]]: + 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] @@ -245,7 +250,7 @@ def _default_resultant_provider(self) -> dict[str, list[float]]: if self._sim_connected.get(f, False) } - def _default_taxel_provider(self) -> dict[str, list[list[float]]]: + def _default_taxel_provider(self) -> TaxelForces: result = {} for finger in FINGER_NAMES: if not self._sim_connected.get(finger, False): @@ -265,7 +270,7 @@ def _default_taxel_provider(self) -> dict[str, list[list[float]]]: @staticmethod def _validate_finger_vectors( - data: dict[str, list[float]], expected_len: int, label: str + data: ResultantForces, expected_len: int, label: str ) -> None: for finger, vec in data.items(): if finger not in FINGER_NAMES: 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 index 9892f168..ea9b78c6 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -1,9 +1,11 @@ """Constants for ORCA tactile sensing.""" +# --------------------------------------------------------------------------- +# Client configuration defaults +# --------------------------------------------------------------------------- + FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] VALID_SENSOR_IDS = set(range(5)) - -# Default hardware settings DEFAULT_SENSOR_PORT = "/dev/ttyACM1" DEFAULT_SENSOR_BAUDRATE = 921600 DEFAULT_FINGER_TO_SENSOR_ID = { @@ -24,7 +26,10 @@ "pinky": "touch-sensor-pinky", } -# Protocol constants +# --------------------------------------------------------------------------- +# 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]) @@ -32,17 +37,70 @@ FUNC_CODE_READ = 0x03 FUNC_CODE_WRITE = 0x10 -# Register addresses +# --------------------------------------------------------------------------- +# Register addresses (used by sensor_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_RESULTING_FORCE_START = 0x0500 -ADDR_RESULTING_FORCE_LENGTH = 168 +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 # fx(int16) + fy(int16) + fz(uint16) +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.""" + +# Hardware slot bit positions in the connected-sensors register. +# Each slot has a fixed (byte_index, bit_position) in the 4-byte register block. +# These describe physical board layout — independent of finger_to_sensor_id mapping. +# The finger_to_sensor_id mapping is applied on top to translate slot → finger name. +SLOT_CONNECTED_BIT_POSITIONS = [(0, 2), (0, 6), (1, 2), (1, 6), (2, 2)] + +# Hardware register addresses for each slot's distal-phalanx taxel count. +# Same as above: fixed board layout, finger mapping applied separately. +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..38dd22e4 --- /dev/null +++ b/orca_core/hardware/sensing/protocol.py @@ -0,0 +1,642 @@ +"""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. + +Naming conventions: + build_* — assemble an outgoing request frame (bytes) + parse_* — validate an incoming frame and extract raw data (bytes) + decode_* — interpret raw bytes into domain objects (dicts of forces) + encode_* — convert domain values into register bytes + extract_* — pull a single field from frame metadata + unpack_* — split a payload into its component parts + compute_* — calculate sizes or indices from configuration +""" +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 + + +# ========================================================================= +# Protocol Constants +# ========================================================================= + +FORCE_DECIMAL_PLACES = 1 +"""Decimal places for rounding decoded force values. + +This is a codec-level choice, not a wire-format specification. The sensor +transmits integer LSB counts; this module converts to Newtons and rounds. +""" + + +# ========================================================================= +# 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: + """Check LRC of an auto-stream frame. Returns True if valid. + + Reconstructs the full frame (header + meta + payload) internally so + the caller doesn't need to know the frame assembly recipe. + + Note: returns bool (not raises) because the auto-stream reader counts + bad-LRC frames as a recoverable statistic rather than aborting. + + Args: + meta: 3 bytes after the AA56 header (reserved + eff_len) + payload: The payload bytes (eff_len bytes) + lrc: The LRC byte to validate against + """ + frame_without_lrc = PROTOCOL_HEADER_AUTO + meta + payload + return calculate_checksum(frame_without_lrc) == lrc + + +def _validate_frame_lrc(frame: bytes, context: str) -> None: + """Validate LRC of a request-response frame. Raises on mismatch.""" + if frame[-1] != calculate_checksum(frame[:-1]): + raise IOError(f"{context} LRC mismatch") + + +# ========================================================================= +# Frame Size Helpers +# ========================================================================= + +def read_response_body_size(count: int) -> int: + """Total bytes after the AA55 header in a read response. + + Args: + count: Number of data bytes requested (same value passed to build_read_request) + """ + return RESPONSE_META_SIZE + count + 1 # meta + data + LRC + + +# ========================================================================= +# Frame Builders +# ========================================================================= + +def _validate_u16(value: int, name: str) -> None: + """Validate that a value fits in a uint16 field.""" + 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 and extract data from a read response frame. + + Frame layout: header(2) + reserved(1) + func(1) + addr(2) + count(2) + data(count) + LRC(1) + + Returns: + The data bytes from the response + + Raises: + IOError: If frame is too short, func code is wrong, or LRC validation fails + """ + 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: + """Extract payload length from write response meta bytes. + + Args: + meta: Exactly 6 bytes (reserved + func + addr + nbytes) + + Returns: + Number of payload bytes that follow the meta + + Raises: + ValueError: If meta is not exactly 6 bytes + """ + 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 (LRC and status check). + + Frame layout: header(2) + reserved(1) + func(1) + addr(2) + nbytes(2) + payload(nbytes) + LRC(1) + + Raises: + IOError: If frame is too short, LRC validation fails, or status byte indicates failure + """ + 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: + """Extract effective length from auto-stream frame meta bytes. + + Args: + meta: 3 bytes after the AA56 header (reserved(1) + eff_len(2)) + + Returns: + Effective payload length (includes error_code byte) + + Raises: + ValueError: If eff_len exceeds MAX_AUTO_FRAME_EFF_LEN (possible corruption) + """ + 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]: + """Unpack auto-stream payload into error code and force data. + + The protocol packs a 1-byte sensor error code (0 = no error) followed + by the actual force data into a single payload. This separates them. + + Returns: + (error_code, force_data) tuple + + Raises: + ValueError: If payload is empty + """ + 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: + """Compute payload size for resultant-only auto-stream mode.""" + return num_sensors * BYTES_PER_RESULTANT + + +def compute_taxel_payload_size( + active_sensors: list[str], num_taxels: dict[str, int], +) -> int: + """Compute payload size for taxel-only auto-stream mode.""" + 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: + """Compute payload size for combined (resultant + taxels) auto-stream mode.""" + 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: + """Compute expected auto-stream payload size for the given streaming mode. + + Args: + mode_resultant: Whether resultant force data is enabled + mode_taxels: Whether taxel data is enabled + active_sensors: List of active finger names + num_taxels: {finger: taxel_count} for each finger + """ + 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: + """Validate that payload data matches expected size. Raises ValueError on mismatch.""" + 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_force_vector(data: bytes, offset: int, width: int) -> ForceVector: + """Unpack a force vector (fx signed, fy signed, fz unsigned) from packed bytes. + + Args: + data: Raw byte buffer + offset: Start position in data + width: Bytes per component (1 for taxels, 2 for resultants) + """ + fx = int.from_bytes(data[offset:offset + width], "little", signed=True) * RESOLUTION_N_PER_LSB + fy = int.from_bytes(data[offset + width:offset + 2 * width], "little", signed=True) * RESOLUTION_N_PER_LSB + fz = int.from_bytes(data[offset + 2 * width:offset + 3 * width], "little", signed=False) * RESOLUTION_N_PER_LSB + return [round(fx, FORCE_DECIMAL_PLACES), round(fy, FORCE_DECIMAL_PLACES), round(fz, FORCE_DECIMAL_PLACES)] + + +def _unpack_resultant(data: bytes, offset: int) -> ForceVector: + """Unpack one resultant force vector: fx(int16), fy(int16), fz(uint16).""" + return _unpack_force_vector(data, offset, width=2) + + +def _unpack_taxel(data: bytes, offset: int) -> ForceVector: + """Unpack one taxel force vector: fx(int8), fy(int8), fz(uint8).""" + return _unpack_force_vector(data, offset, width=1) + + +def decode_resultant_auto( + data: bytes, + active_sensors: list[str], +) -> ResultantForces: + """Decode auto-stream resultant forces (6 bytes/sensor, sequential). + + Args: + data: Raw byte data from auto-stream + active_sensors: Finger names sorted by hardware slot ID ascending. + Auto-stream data arrives in slot order, so this ordering is + required for correct finger-to-data mapping. Ordering is the + caller's responsibility — the codec does not validate it because + it has no access to the slot-ID mapping. + + Returns: + Resultant forces for each active sensor + + Raises: + ValueError: If data size doesn't match expected + """ + 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). + + Args: + data: Raw byte data from auto-stream + active_sensors: Finger names sorted by hardware slot ID ascending. + Auto-stream data arrives in slot order, so this ordering is + required for correct finger-to-data mapping. Ordering is the + caller's responsibility — the codec does not validate it because + it has no access to the slot-ID mapping. + num_taxels: {finger: taxel_count} for each finger + + Returns: + Per-taxel forces for each active sensor + + Raises: + ValueError: If data size doesn't match expected + """ + 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 auto-stream combined format (resultant + taxels interleaved per sensor). + + For each sensor in slot order: resultant(6 bytes) then taxels(3 bytes each), + followed by the next sensor's resultant + taxels, and so on. + + Args: + data: Raw byte data from auto-stream + active_sensors: Finger names sorted by hardware slot ID ascending. + Auto-stream data arrives in slot order, so this ordering is + required for correct finger-to-data mapping. Ordering is the + caller's responsibility — the codec does not validate it because + it has no access to the slot-ID mapping. + num_taxels: {finger: taxel_count} for each finger + + Returns: + Tuple of (resultant forces, per-taxel forces) + + Raises: + ValueError: If data size doesn't match expected + """ + 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: + """Compute the register-block module index for a fingertip (distal phalanx) sensor. + + The resultant force register block contains MODULES_PER_SLOT modules per + sensor slot (proximal, middle, distal, nail). The distal phalanx is at + offset DISTAL_MODULE_OFFSET within each slot's group. + + Args: + sensor_id: Hardware slot ID (0-4) + + Returns: + Module index into the 28-module resultant force register block + """ + 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 full 168-byte resultant force register block (0x0500-0x05A7). + + Used in request-response mode where the full register block is returned. + The block contains 28 modules (MODULES_PER_SLOT per sensor slot: proximal, + middle, distal, nail; plus 8 palm modules), each with 6 bytes (fx, fy, fz). + Use compute_distal_module_index() to get the correct module index for + fingertip sensors. + + Args: + data: Raw 168-byte register block + active_sensors: Finger names to extract + module_indices: {finger: module_idx} — zero-based index into the + 28-module block. Byte offset = module_idx * BYTES_PER_RESULTANT. + For fingertip sensors, use compute_distal_module_index(slot_id) + to get the correct index. + + Returns: + Resultant forces for each active sensor + + Raises: + ValueError: If data is too short + """ + 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 connected-sensors register (4 bytes) into {finger: bool}. + + Args: + data: 4-byte register block + sensor_id_to_finger: {slot_id: finger_name} mapping for all slots + + Returns: + {finger: is_connected} for each slot + + Raises: + ValueError: If data is too short or sensor_id_to_finger doesn't cover all slots + """ + 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 taxel-count register block into {finger: count}. + + Args: + data: 56-byte register block (28 x uint16 little-endian) + sensor_id_to_finger: {slot_id: finger_name} mapping for all slots + + Returns: + {finger: taxel_count} for each slot + + Raises: + ValueError: If data is too short or sensor_id_to_finger doesn't cover all slots + """ + 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)}" + ) + # Length validated above as ADDR_NUM_TAXELS_LENGTH (56), guaranteeing even byte count + 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]) diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index e46ed5f0..2d22db61 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -17,23 +17,45 @@ DEFAULT_SENSOR_PORT, DEFAULT_SENSOR_BAUDRATE, DEFAULT_FINGER_TO_SENSOR_ID, - PROTOCOL_HEADER_REQUEST, PROTOCOL_HEADER_RESPONSE, PROTOCOL_HEADER_AUTO, - PROTOCOL_RESERVED, - FUNC_CODE_READ, - FUNC_CODE_WRITE, + RESPONSE_META_SIZE, + AUTO_FRAME_META_SIZE, ADDR_RESET, ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH, ADDR_NUM_TAXELS_START, ADDR_NUM_TAXELS_LENGTH, - ADDR_RESULTING_FORCE_START, - ADDR_RESULTING_FORCE_LENGTH, + ADDR_RESULTANT_FORCE_START, + RESULTANT_BLOCK_SIZE, ADDR_AUTO_DATA_TYPE, ADDR_AUTO_ENABLE, - AUTO_DATA_RESULTANT, - AUTO_DATA_TAXELS, + 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_resultant_payload_size, + compute_taxel_payload_size, + compute_combined_payload_size, + 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__) @@ -56,35 +78,6 @@ def __init__(self, message: str, bad_lrc: bool = False): self.bad_lrc = bad_lrc -def calculate_checksum(frame: bytes) -> int: - """Calculate checksum for the protocol frame. - - Algorithm: LRC - - Args: - frame: The frame bytes to calculate checksum for (excluding checksum byte) - - Returns: - Checksum value (single byte) - """ - total_sum = sum(frame) - lower_8_bits = total_sum & 0xFF - checksum = (0x100 - lower_8_bits) & 0xFF - - return checksum - -def int_to_little_endian(value: int, num_bytes: int = 2) -> bytes: - """Convert integer to little-endian bytes. - - Args: - value: Integer value to convert - num_bytes: Number of bytes to use (default: 2) - - Returns: - Little-endian byte representation - """ - return value.to_bytes(num_bytes, byteorder='little') - @dataclass class AutoStreamStats: frames_ok: int = 0 @@ -121,6 +114,7 @@ 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))) @@ -265,15 +259,7 @@ def _read_register(self, address: int, count: int = 1, response_timeout_s: float if not self.is_connected: raise OSError("Must call connect() first.") - # Build request frame: 55 AA | reserved | func(0x03=READ) | addr | count | LRC - request = ( - PROTOCOL_HEADER_REQUEST # 55 AA - + int_to_little_endian(PROTOCOL_RESERVED, 1) # 0x00 - + int_to_little_endian(FUNC_CODE_READ, 1) # 0x03 - + int_to_little_endian(address, 2) # Address (little-endian) - + int_to_little_endian(count, 2) # Byte count (little-endian) - ) - request += bytes([calculate_checksum(request)]) + request = build_read_request(address, count) # Clear stale data if not streaming (prevents reading old responses) if not self._is_streaming(): @@ -294,27 +280,12 @@ def _read_register(self, address: int, count: int = 1, response_timeout_s: float continue break # Found AA55 response - # Parse response: AA 55 | meta(6) | data(count) | LRC(1) - meta = self._read_exact(6) # reserved(1) + func(1) + addr(2) + count(2) - data = self._read_exact(count) - lrc = self._read_exact(1) - - # Validate checksum - full = hdr + meta + data + lrc - if full[-1] != calculate_checksum(full[:-1]): - raise IOError("Read response LRC mismatch") - - return data + body = self._read_exact(read_response_body_size(count)) + return parse_read_response(hdr + body) def _skip_auto_frame(self) -> None: """Skip one complete auto-stream frame after having consumed the AA56 header. - Frame format (after AA56 header): - - reserved (1 byte): typically 0x00 - - eff_len (2 bytes, little-endian): length of error_code + payload - - payload (eff_len bytes): error_code(1) + valid_data(eff_len-1) - - LRC (1 byte): checksum - This is called when waiting for a request-response (AA55) frame but an auto-stream (AA56) frame arrives first. We skip it to continue waiting for the AA55 response. @@ -323,15 +294,9 @@ def _skip_auto_frame(self) -> None: IOError: If serial read fails ValueError: If eff_len is unreasonably large (>8KB) """ - _reserved = self._read_exact(1) - eff_len = int.from_bytes(self._read_exact(2), "little") - - # Sanity check: typical payload is 6-200 bytes, max reasonable is ~8KB - if eff_len > 8192: - raise ValueError(f"Invalid eff_len in auto frame: {eff_len} (possible corruption)") - - # Read and discard payload + LRC - _ = self._read_exact(eff_len + 1) + 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: """Read bytes until we find either AA55 (response) or AA56 (auto) header. @@ -407,16 +372,7 @@ def _write_register( if not self.is_connected: raise OSError("Must call connect() first.") - # Build request frame: 55 AA | reserved | func(0x10=WRITE) | addr | len | data | LRC - request = ( - PROTOCOL_HEADER_REQUEST # 55 AA - + int_to_little_endian(PROTOCOL_RESERVED, 1) # 0x00 - + int_to_little_endian(FUNC_CODE_WRITE, 1) # 0x10 - + int_to_little_endian(address, 2) # Address (little-endian) - + int_to_little_endian(len(data), 2) # Data length (little-endian) - + data # Payload - ) - request += bytes([calculate_checksum(request)]) + request = build_write_request(address, data) # Clear stale data if not streaming (prevents reading old responses) if not self._is_streaming(): @@ -440,27 +396,11 @@ def _write_register( # Found AA55 response break - # Parse response: AA 55 | meta(6) | status(1) | LRC(1) - # Meta fields: reserved(1) + func(1) + addr(2) + nbytes(2) - fixed_rest = self._read_exact(6) - fixed = hdr + fixed_rest # Total 8 bytes - - returned_nbytes = int.from_bytes(fixed[6:8], "little") - - # Read status + LRC - rest = self._read_exact(returned_nbytes + 1) - full = fixed + rest - - # Validate checksum - expected = calculate_checksum(full[:-1]) - if full[-1] != expected: - raise IOError("Write response LRC mismatch") - - # Check status byte (first byte of response payload) - if returned_nbytes >= 1: - status = rest[0] - if status != 0: - raise IOError(f"Write failed, status=0x{status:02X}") + # Read and parse response: header(2) + meta(6) + payload(nbytes) + LRC(1) + 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) @@ -477,16 +417,7 @@ def read_connected_sensors(self) -> dict[str, bool]: raise OSError("Must call connect() first.") data = self._read_register(ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH) - if len(data) < 4: - raise ValueError(f'Expected 4 bytes, got {len(data)}') - status = [ - bool(data[0] & (1 << 2)), - bool(data[0] & (1 << 6)), - bool(data[1] & (1 << 2)), - bool(data[1] & (1 << 6)), - bool(data[2] & (1 << 2)), - ] - return {self._sensor_id_to_finger[i]: status[i] for i in range(5)} + return decode_connected_sensors(data, self._sensor_id_to_finger) def read_num_taxels(self) -> dict[str, int]: """Read the number of taxels for each fingertip sensor. @@ -501,13 +432,7 @@ def read_num_taxels(self) -> dict[str, int]: raise OSError("Must call connect() first.") data = self._read_register(ADDR_NUM_TAXELS_START, ADDR_NUM_TAXELS_LENGTH) - taxel_counts = [int.from_bytes(data[i:i+2], byteorder='little') for i in range(0, len(data), 2)] - SLOT_REGISTER_OFFSETS = [0x0034, 0x003C, 0x0044, 0x004C, 0x0054] - distal_indices = { - self._sensor_id_to_finger[slot]: (addr - ADDR_NUM_TAXELS_START) // 2 - for slot, addr in enumerate(SLOT_REGISTER_OFFSETS) - } - return {finger: taxel_counts[idx] for finger, idx in distal_indices.items()} + return decode_num_taxels(data, self._sensor_id_to_finger) def read_auto_data_type(self) -> dict: """Read the auto data type register. @@ -522,18 +447,13 @@ def read_auto_data_type(self) -> dict: raise OSError("Must call connect() first.") data = self._read_register(ADDR_AUTO_DATA_TYPE, 1) - byte_val = data[0] - return { - "raw": f"{byte_val:08b}", - "resulting_force": bool(byte_val & AUTO_DATA_RESULTANT), - "individual_taxels_force": bool(byte_val & AUTO_DATA_TAXELS), - } + return decode_auto_data_type(data) def _read_raw_resultant(self) -> dict[str, list[float]]: """Read raw resultant forces from hardware (no offset application). Subclasses (e.g. MockSensorClient) override this to return simulated - data. The public read_resulting_force() method calls this, then applies + data. The public read_resultant_force() method calls this, then applies zeroing offsets. Returns: @@ -545,15 +465,18 @@ def _read_raw_resultant(self) -> dict[str, list[float]]: self._sensor_config = self._get_configuration() except Exception as e: logger.error(f"Failed to get configuration: {e}") - # Fall back to static parsing - data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) - return self._parse_resultant_force_block(data) - - data = self._read_register(ADDR_RESULTING_FORCE_START, ADDR_RESULTING_FORCE_LENGTH) - return self._parse_resultant_force_dynamic(data, self._sensor_config) + # Fall back to static parsing using default module indices + 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._sensor_config.active_sensors, self._sensor_config.module_indices, + ) - def read_resulting_force(self) -> dict[str, list[float]]: - """Read resulting force from all connected fingertip sensors. + def read_resultant_force(self) -> dict[str, list[float]]: + """Read resultant force from all connected fingertip sensors. Calls _read_raw_resultant() for data, then applies zeroing offsets. Subclasses should override _read_raw_resultant(), not this method. @@ -602,22 +525,13 @@ def _get_configuration(self) -> SensorConfiguration: for finger in FINGER_NAMES: if connected.get(finger, False): sensor_id = self._finger_to_sensor_id[finger] - module_indices[finger] = sensor_id * 4 + 2 + module_indices[finger] = compute_distal_module_index(sensor_id) # Calculate expected payload sizes - num_active = sum(1 for c in connected.values() if c) - expected_resultant = num_active * 6 # Each sensor: fx(2) + fy(2) + fz(2) = 6 bytes - - # Calculate taxel payload size: sum of taxels for active sensors * 3 bytes per taxel - # Each taxel sends 3 bytes: fx(1) + fy(1) + fz(1) as int8 values - expected_taxels = sum( - num_taxels.get(finger, 0) * 3 - for finger, is_connected in connected.items() - if is_connected - ) - - # Combined mode: resultant forces followed by taxels - expected_combined = expected_resultant + expected_taxels + active = [f for f in FINGER_NAMES if connected.get(f, False)] + expected_resultant = compute_resultant_payload_size(len(active)) + expected_taxels = compute_taxel_payload_size(active, num_taxels) + expected_combined = compute_combined_payload_size(active, num_taxels) config = SensorConfiguration( connected=connected, @@ -700,192 +614,6 @@ def _reconfigure(self, force: bool = False) -> bool: logger.error(f"Reconfiguration failed: {e}") raise - def _parse_auto_stream_compact(self, data: bytes, config: SensorConfiguration) -> dict[str, list[float]]: - """Parse auto-stream compact format (active sensors only, sequential). - - Auto-stream mode sends only data for connected sensors in sequential order. - Each sensor is 6 bytes: fx(2) + fy(2) + fz(2). - - For example, if only thumb and middle are connected: - - Bytes 0-5: thumb (fx, fy, fz) - - Bytes 6-11: middle (fx, fy, fz) - - Args: - data: Raw byte data from auto-stream (6 * num_active_sensors bytes) - config: Current sensor configuration - - Returns: - Dictionary mapping active finger names to [fx, fy, fz] force vectors - Uses sparse dict format (only active sensors included) - - Raises: - ValueError: If data size doesn't match expected size - """ - RESOLUTION_N_PER_LSB = 0.1 - BYTES_PER_SENSOR = 6 - - expected_size = config.num_active_sensors * BYTES_PER_SENSOR - if len(data) != expected_size: - raise ValueError( - f"Auto-stream compact data size mismatch: " - f"expected {expected_size} bytes ({config.num_active_sensors} sensors), " - f"got {len(data)} bytes" - ) - - result = {} - for i, finger in enumerate(config.active_sensors): - offset = i * BYTES_PER_SENSOR - - # Parse force data sequentially - fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB - - result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] - - return result - - def _parse_taxels_compact(self, data: bytes, config: SensorConfiguration) -> dict[str, list[list[float]]]: - """Parse auto-stream taxels-only format (active sensors only, sequential). - - Auto-stream taxels mode sends only taxel data for connected sensors in sequential order. - Each taxel is 3 bytes: fx(int8) + fy(int8) + fz(int8). - - For example, if thumb (127 taxels) and index (52 taxels) are connected: - - Bytes 0-380: thumb taxels (127 * 3 = 381 bytes) - - Bytes 381-536: index taxels (52 * 3 = 156 bytes) - - Args: - data: Raw byte data from auto-stream - config: Current sensor configuration - - Returns: - Dictionary mapping active finger names to list of taxel force vectors [fx, fy, fz] - - Raises: - ValueError: If data size doesn't match expected size - """ - RESOLUTION_N_PER_LSB = 0.1 - BYTES_PER_TAXEL = 3 - - expected_size = config.expected_payload_size_taxels - if len(data) != expected_size: - raise ValueError( - f"Auto-stream taxels data size mismatch: " - f"expected {expected_size} bytes, got {len(data)} bytes" - ) - - result = {} - offset = 0 - for finger in config.active_sensors: - taxel_count = config.num_taxels.get(finger, 0) - taxels = [] - for _ in range(taxel_count): - # Each taxel: fx(int8), fy(int8), fz(uint8) - fx = int.from_bytes(data[offset:offset+1], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fy = int.from_bytes(data[offset+1:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fz = int.from_bytes(data[offset+2:offset+3], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB - taxels.append([round(fx, 2), round(fy, 2), round(fz, 2)]) - offset += BYTES_PER_TAXEL - result[finger] = taxels - - return result - - def _parse_combined_compact( - self, data: bytes, config: SensorConfiguration - ) -> tuple[dict[str, list[float]], dict[str, list[list[float]]]]: - """Parse auto-stream combined format (resultant + taxels for active sensors). - - Combined mode sends data interleaved per sensor: - [sensor1_resultant][sensor1_taxels][sensor2_resultant][sensor2_taxels]... - - Args: - data: Raw byte data from auto-stream - config: Current sensor configuration - - Returns: - Tuple of (resultant_forces, taxels): - - resultant_forces: Dict mapping finger names to [fx, fy, fz] - - taxels: Dict mapping finger names to list of taxel values - - Raises: - ValueError: If data size doesn't match expected size - """ - BYTES_PER_RESULTANT = 6 - BYTES_PER_TAXEL = 3 - RESOLUTION_RESULTANT = 0.1 - RESOLUTION_TAXEL = 0.1 - - expected_size = config.expected_payload_size_combined - if len(data) != expected_size: - raise ValueError( - f"Auto-stream combined data size mismatch: " - f"expected {expected_size} bytes, got {len(data)} bytes" - ) - - offset = 0 - resultant_forces = {} - taxels = {} - - for finger in config.active_sensors: - # Parse resultant (6 bytes: fx:int16, fy:int16, fz:uint16) - fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_RESULTANT - fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_RESULTANT - fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_RESULTANT - resultant_forces[finger] = [round(fx, 1), round(fy, 1), round(fz, 1)] - offset += BYTES_PER_RESULTANT - - # Parse taxels (taxel_count × 3 bytes: fx:int8, fy:int8, fz:uint8) - taxel_count = config.num_taxels.get(finger, 0) - finger_taxels = [] - for _ in range(taxel_count): - tfx = int.from_bytes(data[offset:offset+1], byteorder='little', signed=True) * RESOLUTION_TAXEL - tfy = int.from_bytes(data[offset+1:offset+2], byteorder='little', signed=True) * RESOLUTION_TAXEL - tfz = int.from_bytes(data[offset+2:offset+3], byteorder='little', signed=False) * RESOLUTION_TAXEL - finger_taxels.append([round(tfx, 2), round(tfy, 2), round(tfz, 2)]) - offset += BYTES_PER_TAXEL - taxels[finger] = finger_taxels - - return resultant_forces, taxels - - def _parse_resultant_force_dynamic(self, data: bytes, config: SensorConfiguration) -> dict[str, list[float]]: - """Parse resultant force data using dynamic configuration (offset-based). - - This parser is used for request-response mode where the full 168-byte block - is returned with all 28 modules. It adapts to the actual connected sensors, - returning data only for available sensors. Uses sparse dict format. - - Args: - data: Raw byte data from sensor (168 bytes for full block) - config: Current sensor configuration - - Returns: - Dictionary mapping active finger names to [fx, fy, fz] force vectors - Only includes sensors that are currently connected - - Raises: - ValueError: If data is too short for expected configuration - """ - RESOLUTION_N_PER_LSB = 0.1 - - if len(data) < ADDR_RESULTING_FORCE_LENGTH: - raise ValueError(f"Resultant force block too short: {len(data)} bytes") - - result = {} - for finger in config.active_sensors: - # Get module index for this sensor from configuration - module_idx = config.module_indices[finger] - offset = module_idx * 6 # Each module force is 6 bytes (fx, fy, fz) - - # Parse force data from fixed offsets in full block - fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB - - result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] - - return result - def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: """Configure which data types to include in auto stream. @@ -900,8 +628,7 @@ def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> No if not self.is_connected: raise OSError("Must call connect() first.") - val = (AUTO_DATA_RESULTANT if resultant else 0) | (AUTO_DATA_TAXELS if taxels else 0) - self._write_register(ADDR_AUTO_DATA_TYPE, bytes([val])) + self._write_register(ADDR_AUTO_DATA_TYPE, encode_auto_data_type(resultant, taxels)) def enable_auto_data_transmission(self) -> None: @@ -913,7 +640,7 @@ def enable_auto_data_transmission(self) -> None: if not self.is_connected: raise OSError("Must call connect() first.") - self._write_register(ADDR_AUTO_ENABLE, bytes([0x01])) + self._write_register(ADDR_AUTO_ENABLE, REGISTER_ENABLE) def disable_auto_data_transmission(self) -> None: """Disable automatic data transmission mode. @@ -924,7 +651,7 @@ def disable_auto_data_transmission(self) -> None: if not self.is_connected: raise OSError("Must call connect() first.") - self._write_register(ADDR_AUTO_ENABLE, bytes([0x00])) + self._write_register(ADDR_AUTO_ENABLE, REGISTER_DISABLE) def reboot(self) -> None: @@ -936,7 +663,7 @@ def reboot(self) -> None: if not self.is_connected: raise OSError("Must call connect() first.") - self._write_register(ADDR_RESET, bytes([0x01])) + self._write_register(ADDR_RESET, REGISTER_ENABLE) def get_auto_latest(self): """Get the most recently parsed auto-stream resultant force data (thread-safe). @@ -1119,9 +846,9 @@ def _apply_taxel_offsets(self, taxels: dict) -> None: if i >= len(finger_offsets): break off = finger_offsets[i] - taxel[0] = round(taxel[0] - off[0], 2) - taxel[1] = round(taxel[1] - off[1], 2) - taxel[2] = round(max(0, taxel[2] - off[2]), 2) + 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.""" @@ -1193,7 +920,7 @@ def _resync_to_auto_header(self) -> None: b1 = self._read_exact(1) while self._auto_running.is_set(): b2 = self._read_exact(1) - if b1 == bytes([0xAA]) and b2 == bytes([0x56]): + if b1 + b2 == PROTOCOL_HEADER_AUTO: return # Found the header # Slide window: b2 becomes new b1 b1 = b2 @@ -1201,52 +928,14 @@ def _resync_to_auto_header(self) -> None: # If we exit the loop, auto stream was stopped raise IOError("Auto stream stopped during resync") - def _check_lrc(self, frame_wo_lrc: bytes, lrc_byte: int) -> bool: - expected = calculate_checksum(frame_wo_lrc) - return expected == lrc_byte - - - def _parse_resultant_force_block(self, data: bytes) -> dict[str, list[float]]: - """Parse a resultant force data block from the sensor. - - Module index calculation (module_idx = i * 4 + 2): - - Each finger has 4 potential modules: proximal(0), middle(1), distal(2), nail(3) - - We only use fingertip sensors, which are the distal phalanx (index 2) - - For thumb: module_idx = 0*4+2 = 2 (byte offset = 2*6 = 12) - - For index: module_idx = 1*4+2 = 6 (byte offset = 6*6 = 36) - - etc. - - Args: - data: Raw byte data containing resultant forces (168 bytes for 28 modules) - - Returns: - Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons - """ - RESOLUTION_N_PER_LSB = 0.1 - if len(data) < ADDR_RESULTING_FORCE_LENGTH: - raise ValueError(f"Resultant force block too short: {len(data)} bytes") - - result = {} - for finger in FINGER_NAMES: - sensor_id = self._finger_to_sensor_id[finger] - module_idx = sensor_id * 4 + 2 - offset = module_idx * 6 # Each module force is 6 bytes (fx, fy, fz) - - fx = int.from_bytes(data[offset:offset+2], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fy = int.from_bytes(data[offset+2:offset+4], byteorder='little', signed=True) * RESOLUTION_N_PER_LSB - fz = int.from_bytes(data[offset+4:offset+6], byteorder='little', signed=False) * RESOLUTION_N_PER_LSB - result[finger] = [round(float(fx), 1), round(float(fy), 1), round(float(fz), 1)] - return result - def _get_expected_payload_size(self, config: SensorConfiguration) -> int: """Get expected payload size based on current streaming mode.""" - if self._auto_mode_resultant and self._auto_mode_taxels: - return config.expected_payload_size_combined - elif self._auto_mode_resultant: - return config.expected_payload_size_resultant - elif self._auto_mode_taxels: - return config.expected_payload_size_taxels - return 0 + return compute_expected_payload_size( + self._auto_mode_resultant, + self._auto_mode_taxels, + config.active_sensors, + config.num_taxels, + ) def _acquire_frame( self, @@ -1274,11 +963,9 @@ def _acquire_frame( # Find and consume AA 56 header self._resync_to_auto_header() - # Read frame metadata - reserved = self._read_exact(1) - eff_len = int.from_bytes(self._read_exact(2), "little") - - # Read payload and checksum + # Read frame metadata, payload, and checksum + 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] @@ -1290,13 +977,11 @@ def _acquire_frame( self._last_frame_debug_print = now # Validate frame integrity - frame_wo_lrc = bytes([0xAA, 0x56]) + reserved + int_to_little_endian(eff_len, 2) + payload - if not self._check_lrc(frame_wo_lrc, lrc): + if not validate_auto_frame_lrc(meta, payload, lrc): raise FrameError("LRC mismatch", bad_lrc=True) - # Split error code and valid data - err_code = payload[0] - valid = payload[1:] + # Split error code and force data + err_code, valid = unpack_auto_payload(payload) # Update serial-specific stats with self._auto_lock: @@ -1331,16 +1016,19 @@ def _acquire_frame( f"Unexpected payload: {len(valid)} bytes, expected {expected_size}" ) + cfg = self._sensor_config if parse_resultant and parse_taxels: - parsed_resultant, parsed_taxels = self._parse_combined_compact( - valid, self._sensor_config + parsed_resultant, parsed_taxels = decode_combined_auto( + valid, cfg.active_sensors, cfg.num_taxels, ) elif parse_resultant: - parsed_resultant = self._parse_auto_stream_compact(valid, self._sensor_config) + parsed_resultant = decode_resultant_auto(valid, cfg.active_sensors) parsed_taxels = None elif parse_taxels: parsed_resultant = None - parsed_taxels = self._parse_taxels_compact(valid, self._sensor_config) + parsed_taxels = decode_taxels_auto( + valid, cfg.active_sensors, cfg.num_taxels, + ) else: parsed_resultant = None parsed_taxels = None @@ -1447,7 +1135,7 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se In auto-stream mode, the sensor continuously broadcasts force data at ~1kHz without requiring request-response polling. This provides much lower latency - and higher throughput than repeatedly calling read_resulting_force(). + and higher throughput than repeatedly calling read_resultant_force(). The data is read by a background thread and made available via: - get_auto_latest(): for resultant force data @@ -1579,62 +1267,3 @@ def __enter__(self): def __exit__(self, *args): self.disconnect() - - -if __name__ == "__main__": - import sys - - sensor_client = SensorClient(port="/dev/ttyACM0", baudrate=921600) - - sensor_client.connect() - print(sensor_client._sensor_config) - exit() - try: - print("connected sensors:", sensor_client.read_connected_sensors()) - print("num taxels:", sensor_client.read_num_taxels()) - print("config:", sensor_client.get_sensor_configuration()) - - # Parse command line for mode selection - mode = sys.argv[1] if len(sys.argv) > 1 else "resultant" - - if mode == "resultant": - print("\n=== Resultant Force Only Mode ===") - sensor_client.start_auto_stream(resultant=True, taxels=False) - for _ in range(100): - forces, ts = sensor_client.get_auto_latest() - if forces is not None: - print(f"[{ts:.3f}] forces: {forces}") - time.sleep(0.05) - - elif mode == "taxels": - print("\n=== Taxels Only Mode ===") - sensor_client.start_auto_stream(resultant=False, taxels=True) - for _ in range(100): - taxels, ts = sensor_client.get_auto_latest_taxels() - if taxels is not None: - # Print summary: count and first taxel [fx, fy, fz] per finger - summary = {f: (len(vals), vals[0] if vals else []) for f, vals in taxels.items()} - print(f"[{ts:.3f}] taxels (count, first): {summary}") - time.sleep(0.05) - - elif mode == "combined": - print("\n=== Combined Mode (Resultant + Taxels) ===") - sensor_client.start_auto_stream(resultant=True, taxels=True) - for _ in range(100): - forces, taxels, ts = sensor_client.get_auto_latest_all() - if forces is not None: - taxel_summary = {f: len(vals) for f, vals in taxels.items()} if taxels else {} - print(f"[{ts:.3f}] forces: {forces}, taxel_counts: {taxel_summary}") - time.sleep(0.05) - - else: - print(f"Unknown mode: {mode}. Use 'resultant', 'taxels', or 'combined'") - - print("\nstats:", sensor_client.get_auto_stats()) - - finally: - try: - sensor_client.stop_auto_stream() - except Exception: - pass - sensor_client.disconnect() diff --git a/scripts/tactile_sensing_ui/tactile_ui.py b/scripts/tactile_sensing_ui/tactile_ui.py index 97738620..eac059a2 100755 --- a/scripts/tactile_sensing_ui/tactile_ui.py +++ b/scripts/tactile_sensing_ui/tactile_ui.py @@ -273,7 +273,7 @@ def forces(): client = get_sensor_client() if not client.is_connected: return jsonify({'error': 'Not connected'}), 400 - forces = client.read_resulting_force() + forces = client.read_resultant_force() return jsonify(forces) except Exception as e: return jsonify({'error': str(e)}), 400 @@ -294,7 +294,7 @@ def refresh(): connected = client.read_connected_sensors() taxels = client.read_num_taxels() auto_data = client.read_auto_data_type() - forces = client.read_resulting_force() + forces = client.read_resultant_force() return jsonify({ 'connected': True, diff --git a/tests/test_protocol.py b/tests/test_protocol.py new file mode 100644 index 00000000..b9e7bb80 --- /dev/null +++ b/tests/test_protocol.py @@ -0,0 +1,527 @@ +"""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, + 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, +) +from orca_core.hardware.sensing.constants import ( + 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, + MIN_READ_RESPONSE_SIZE, + MIN_WRITE_RESPONSE_SIZE, + MODULES_PER_SLOT, + DISTAL_MODULE_OFFSET, +) + + +# --------------------------------------------------------------------------- +# Checksum +# --------------------------------------------------------------------------- + +class TestCalculateChecksum: + def test_known_value(self): + assert calculate_checksum(b"\x01\x02\x03") == 0xFA + + def test_round_trip(self): + frame = b"\xAA\x55\x00\x03\x10\x00\x04\x00" + checksum = calculate_checksum(frame) + assert (sum(frame) + checksum) & 0xFF == 0 + + def test_empty_frame(self): + assert calculate_checksum(b"") == 0 + + def test_single_byte(self): + assert calculate_checksum(b"\x01") == 0xFF + + +class TestValidateAutoFrameLrc: + def test_valid_frame(self): + meta = b"\x00" + (3).to_bytes(2, "little") + payload = b"\x00\x01\x02" + frame_wo_lrc = b"\xAA\x56" + meta + payload + lrc = calculate_checksum(frame_wo_lrc) + assert validate_auto_frame_lrc(meta, payload, lrc) is True + + def test_invalid_lrc(self): + meta = b"\x00" + (3).to_bytes(2, "little") + payload = b"\x00\x01\x02" + assert validate_auto_frame_lrc(meta, payload, 0xFF) is False + + +# --------------------------------------------------------------------------- +# Frame Size Helpers +# --------------------------------------------------------------------------- + +class TestReadResponseBodySize: + def test_known_value(self): + # count=4 → meta(6) + data(4) + LRC(1) = 11 + assert read_response_body_size(4) == 11 + + def test_single_byte(self): + assert read_response_body_size(1) == 8 + + +# --------------------------------------------------------------------------- +# Frame Builders +# --------------------------------------------------------------------------- + +class TestBuildReadRequest: + def test_structure(self): + frame = build_read_request(address=0x0010, count=4) + assert frame[:2] == PROTOCOL_HEADER_REQUEST + assert frame[2] == 0x00 # reserved + assert frame[3] == FUNC_CODE_READ + assert int.from_bytes(frame[4:6], "little") == 0x0010 + assert int.from_bytes(frame[6:8], "little") == 4 + + def test_lrc_valid(self): + frame = build_read_request(address=0x0010, count=4) + assert calculate_checksum(frame[:-1]) == frame[-1] + + +class TestBuildWriteRequest: + def test_structure(self): + frame = build_write_request(address=0x0017, data=b"\x01") + assert frame[:2] == PROTOCOL_HEADER_REQUEST + assert frame[2] == 0x00 # reserved + 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 # data byte + + def test_lrc_valid(self): + frame = build_write_request(address=0x0017, data=b"\x01") + assert calculate_checksum(frame[:-1]) == frame[-1] + + +# --------------------------------------------------------------------------- +# Frame Parsers — response frames +# --------------------------------------------------------------------------- + +def _build_read_response(data: bytes) -> bytes: + """Helper: build a valid read response frame for testing.""" + 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: + """Helper: build a valid write response frame for testing.""" + 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)]) + + +class TestParseReadResponse: + def test_extracts_data(self): + frame = _build_read_response(b"\xAB\xCD\xEF\x01") + assert parse_read_response(frame) == b"\xAB\xCD\xEF\x01" + + def test_single_byte(self): + frame = _build_read_response(b"\x42") + assert parse_read_response(frame) == b"\x42" + + def test_bad_lrc_raises(self): + frame = bytearray(_build_read_response(b"\x01\x02")) + frame[-1] ^= 0xFF # corrupt LRC + with pytest.raises(IOError, match="LRC mismatch"): + parse_read_response(bytes(frame)) + + def test_too_short_raises(self): + with pytest.raises(IOError, match="too short"): + parse_read_response(b"\xAA\x55\x00") + + def test_wrong_func_code_raises(self): + 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_wrong_header_raises(self): + frame = bytearray(_build_read_response(b"\x01\x02")) + frame[0:2] = PROTOCOL_HEADER_AUTO # AA 56 instead of AA 55 + frame[-1] = calculate_checksum(bytes(frame[:-1])) # fix LRC + with pytest.raises(IOError, match="Expected response header"): + parse_read_response(bytes(frame)) + + +class TestParseWriteResponse: + def test_success(self): + frame = _build_write_response(status=0x00) + parse_write_response(frame) # should not raise + + def test_failure_status_raises(self): + frame = _build_write_response(status=0x01) + with pytest.raises(IOError, match="Write failed"): + parse_write_response(frame) + + def test_bad_lrc_raises(self): + frame = bytearray(_build_write_response(status=0x00)) + frame[-1] ^= 0xFF + with pytest.raises(IOError, match="LRC mismatch"): + parse_write_response(bytes(frame)) + + def test_too_short_raises(self): + with pytest.raises(IOError, match="too short"): + parse_write_response(b"\xAA\x55\x00") + + def test_truncated_payload_raises(self): + # Build a frame that 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_wrong_header_raises(self): + 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)) + + +class TestExtractWriteResponseDataLength: + def test_known_value(self): + # 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_wrong_size_raises(self): + with pytest.raises(ValueError, match="must be 6 bytes"): + extract_write_response_data_length(b"\x00\x00\x00\x00") + + +# --------------------------------------------------------------------------- +# Frame Parsers — auto-stream frames +# --------------------------------------------------------------------------- + +class TestExtractAutoFrameEffLen: + def test_known_value(self): + # reserved(1) + eff_len(2 LE) = 3 bytes + meta = b"\x00" + (42).to_bytes(2, "little") + assert extract_auto_frame_eff_len(meta) == 42 + + def test_max_valid(self): + meta = b"\x00" + MAX_AUTO_FRAME_EFF_LEN.to_bytes(2, "little") + assert extract_auto_frame_eff_len(meta) == MAX_AUTO_FRAME_EFF_LEN + + def test_exceeds_max_raises(self): + 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) + + +class TestSplitAutoPayload: + def test_splits_error_code_and_data(self): + err, data = unpack_auto_payload(b"\x00\x01\x02\x03") + assert err == 0 + assert data == b"\x01\x02\x03" + + def test_nonzero_error_code(self): + err, data = unpack_auto_payload(b"\x05\xAB") + assert err == 5 + assert data == b"\xAB" + + def test_error_code_only(self): + err, data = unpack_auto_payload(b"\x01") + assert err == 1 + assert data == b"" + + +# --------------------------------------------------------------------------- +# Payload Size Computation +# --------------------------------------------------------------------------- + +class TestPayloadSizeComputation: + def test_resultant_size(self): + assert compute_resultant_payload_size(3) == 3 * BYTES_PER_RESULTANT + + def test_resultant_size_zero(self): + assert compute_resultant_payload_size(0) == 0 + + def test_taxel_size(self): + active = ["thumb", "index"] + num_taxels = {"thumb": 51, "index": 87} + assert compute_taxel_payload_size(active, num_taxels) == (51 + 87) * BYTES_PER_TAXEL + + def test_taxel_size_missing_finger_raises(self): + with pytest.raises(KeyError): + compute_taxel_payload_size(["thumb"], {"index": 87}) + + def test_combined_size(self): + 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 + + +# --------------------------------------------------------------------------- +# Module Index Computation +# --------------------------------------------------------------------------- + +class TestComputeDistalModuleIndex: + def test_slot_zero(self): + assert compute_distal_module_index(0) == DISTAL_MODULE_OFFSET + + def test_slot_four(self): + assert compute_distal_module_index(4) == 4 * MODULES_PER_SLOT + DISTAL_MODULE_OFFSET + + +# --------------------------------------------------------------------------- +# Payload Decoders — auto-stream +# --------------------------------------------------------------------------- + +class TestDecodeResultantAuto: + def test_known_values(self): + data = struct.pack(" SensorClient: - """Create a SensorClient without connecting (for calling parse methods).""" - client = SensorClient.__new__(SensorClient) - return client - - # --------------------------------------------------------------------------- -# Test 1: Mock client — resultant forces shape +# Mock client — resultant forces # --------------------------------------------------------------------------- class TestMockResultantForces: - def test_shape_all_fingers(self): + def test_values_round_trip(self): + """Values set via set_mock_forces come back through the stream.""" mock = MockSensorClient(connected_sensors=ALL_FINGERS) mock.connect() - forces = {f: [1.0, -0.5, 2.0] for f in ALL_FINGERS} - mock.set_mock_forces(forces) + mock.set_mock_forces({ + "thumb": [1.5, -2.0, 3.0], + "index": [0.0, 0.0, 0.0], + "middle": [-1.0, 0.5, 10.0], + }) mock.start_auto_stream(resultant=True, taxels=False) - time.sleep(0.05) - - result, ts = mock.get_auto_latest() + result, ts = _poll_auto_latest(mock) mock.stop_auto_stream() mock.disconnect() - assert result is not None assert ts is not None - assert set(result.keys()) == set(ALL_FINGERS) - for finger in ALL_FINGERS: - assert len(result[finger]) == 3 - assert all(isinstance(v, float) for v in result[finger]) - - def test_values_match(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - mock.set_mock_forces({"thumb": [1.5, -2.0, 3.0], "index": [0.0, 0.0, 0.0]}) - mock.start_auto_stream(resultant=True, taxels=False) - time.sleep(0.05) - - result, _ = mock.get_auto_latest() - mock.stop_auto_stream() - mock.disconnect() - assert result["thumb"] == [1.5, -2.0, 3.0] assert result["index"] == [0.0, 0.0, 0.0] + assert result["middle"] == [-1.0, 0.5, 10.0] + # Fingers without explicit mock data get default [1.0, 1.0, 1.0] + assert result["ring"] == [1.0, 1.0, 1.0] - def test_subset_of_fingers(self): + def test_subset_only_returns_connected(self): + """Only connected sensors appear in output.""" subset = ["thumb", "pinky"] mock = MockSensorClient(connected_sensors=subset) mock.connect() mock.set_mock_forces({"thumb": [1.0, 0.0, 0.0], "pinky": [0.0, 1.0, 0.0]}) mock.start_auto_stream(resultant=True, taxels=False) - time.sleep(0.05) - - result, _ = mock.get_auto_latest() + result, _ = _poll_auto_latest(mock) mock.stop_auto_stream() mock.disconnect() assert set(result.keys()) == set(subset) + assert result["thumb"] == [1.0, 0.0, 0.0] + assert result["pinky"] == [0.0, 1.0, 0.0] # --------------------------------------------------------------------------- -# Test 2: Mock client — taxel data shape +# Mock client — taxel data # --------------------------------------------------------------------------- class TestMockTaxelData: - def test_shape_all_fingers(self): + def test_taxel_counts_match_sensor_models(self): + """Each finger returns the expected number of taxels.""" mock = MockSensorClient(connected_sensors=ALL_FINGERS) mock.connect() mock.start_auto_stream(resultant=False, taxels=True) - time.sleep(0.05) - - result, ts = mock.get_auto_latest_taxels() + result, _ = _poll_auto_latest_taxels(mock) mock.stop_auto_stream() mock.disconnect() - assert result is not None assert set(result.keys()) == set(ALL_FINGERS) for finger in ALL_FINGERS: assert len(result[finger]) == EXPECTED_TAXEL_COUNTS[finger], ( f"{finger}: expected {EXPECTED_TAXEL_COUNTS[finger]} taxels, " f"got {len(result[finger])}" ) - for taxel in result[finger]: - assert len(taxel) == 3 - assert all(isinstance(v, float) for v in taxel) # --------------------------------------------------------------------------- -# Test 3: Mock client — combined mode shape +# Mock client — combined mode # --------------------------------------------------------------------------- class TestMockCombinedMode: - def test_shape(self): + def test_both_resultant_and_taxels_returned(self): mock = MockSensorClient(connected_sensors=ALL_FINGERS) mock.connect() mock.set_mock_forces({f: [1.0, 0.0, 0.5] for f in ALL_FINGERS}) mock.start_auto_stream(resultant=True, taxels=True) - time.sleep(0.05) - - forces, taxels, ts = mock.get_auto_latest_all() + forces, taxels, ts = _poll_auto_latest_all(mock) mock.stop_auto_stream() mock.disconnect() - assert forces is not None - assert taxels is not None assert set(forces.keys()) == set(ALL_FINGERS) assert set(taxels.keys()) == set(ALL_FINGERS) for finger in ALL_FINGERS: - assert len(forces[finger]) == 3 + assert forces[finger] == [1.0, 0.0, 0.5] assert len(taxels[finger]) == EXPECTED_TAXEL_COUNTS[finger] # --------------------------------------------------------------------------- -# Test 4: Payload size calculation +# Mock client — provider injection # --------------------------------------------------------------------------- -class TestPayloadSize: - def test_all_sensors(self): - config = _make_config(ALL_FINGERS) - total_taxels = 51 + 87 + 87 + 87 + 51 # 363 - assert config.expected_payload_size_resultant == 5 * 6 # 30 - assert config.expected_payload_size_taxels == total_taxels * 3 # 1089 - assert config.expected_payload_size_combined == 30 + total_taxels * 3 # 1119 - - def test_two_sensors(self): - config = _make_config(["thumb", "index"]) - assert config.expected_payload_size_resultant == 2 * 6 # 12 - assert config.expected_payload_size_taxels == (51 + 87) * 3 # 414 - assert config.expected_payload_size_combined == 12 + 414 # 426 - - def test_single_sensor(self): - config = _make_config(["pinky"]) - assert config.expected_payload_size_resultant == 6 - assert config.expected_payload_size_taxels == 51 * 3 # 153 - assert config.expected_payload_size_combined == 6 + 153 # 159 - - def test_no_sensors(self): - config = _make_config([]) - assert config.expected_payload_size_resultant == 0 - assert config.expected_payload_size_taxels == 0 - assert config.expected_payload_size_combined == 0 - - -# --------------------------------------------------------------------------- -# Test 5: Parse resultant compact — known bytes -# --------------------------------------------------------------------------- - -class TestParseResultantCompact: - def test_known_values(self): - client = _sensor_client_instance() - config = _make_config(["thumb"]) - - # fx=100 (10.0N), fy=-50 (-5.0N), fz=200 (20.0N) - data = struct.pack(" 0 + assert result["thumb"][0] > 0 # Provider was called at least once - with pytest.raises(ValueError, match="size mismatch"): - client._parse_auto_stream_compact(b"\x00" * 5, config) + def test_custom_taxel_provider(self): + marker = [[99.0, 88.0, 77.0]] + mock = MockSensorClient( + connected_sensors=["thumb"], + taxel_provider=lambda: {"thumb": marker}, + ) + mock.connect() + mock.start_auto_stream(resultant=False, taxels=True) + result, _ = _poll_auto_latest_taxels(mock) + mock.stop_auto_stream() + mock.disconnect() - with pytest.raises(ValueError, match="size mismatch"): - client._parse_auto_stream_compact(b"\x00" * 7, config) + assert result["thumb"] == marker # --------------------------------------------------------------------------- -# Test 6: Parse taxels compact — known bytes +# Mock client — dynamic reconfiguration # --------------------------------------------------------------------------- -class TestParseTaxelsCompact: - def test_known_values(self): - client = _sensor_client_instance() - # Use small taxel count for test - config = _make_config(["thumb"], taxel_counts={"thumb": 2}) - - # taxel 0: fx=10 (1.0N), fy=-5 (-0.5N), fz=20 (2.0N) - # taxel 1: fx=0, fy=0, fz=50 (5.0N) - data = struct.pack("bbB", 10, -5, 20) + struct.pack("bbB", 0, 0, 50) - result = client._parse_taxels_compact(data, config) - - 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] +class TestDynamicReconfiguration: + def test_simulate_dropout_removes_sensor(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() + assert mock._sensor_config.num_active_sensors == 5 - def test_correct_taxel_count(self): - client = _sensor_client_instance() - config = _make_config(["thumb"], taxel_counts={"thumb": 51}) + mock.simulate_dropout(["index", "ring"]) + assert mock._sensor_config.num_active_sensors == 3 + assert "index" not in mock._sensor_config.active_sensors + assert "ring" not in mock._sensor_config.active_sensors - data = b"\x00" * (51 * 3) - result = client._parse_taxels_compact(data, config) + def test_set_connected_sensors_updates_config(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() - assert len(result["thumb"]) == 51 + mock.set_connected_sensors(["thumb"]) + assert mock._sensor_config.active_sensors == ["thumb"] + assert mock._sensor_config.num_active_sensors == 1 - def test_wrong_size_raises(self): - client = _sensor_client_instance() - config = _make_config(["thumb"], taxel_counts={"thumb": 2}) + def test_dropout_clears_mock_data(self): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock.connect() + mock.set_mock_forces({"index": [5.0, 0.0, 0.0]}) - with pytest.raises(ValueError, match="size mismatch"): - client._parse_taxels_compact(b"\x00" * 5, config) + mock.simulate_dropout(["index"]) + # index mock data should be cleared + assert "index" not in mock._mock_forces # --------------------------------------------------------------------------- -# Test 7: Parse combined compact — known bytes +# Offset logic # --------------------------------------------------------------------------- -class TestParseCombinedCompact: - def test_interleaved_format(self): - client = _sensor_client_instance() - config = _make_config(["thumb", "index"], taxel_counts={"thumb": 1, "index": 1}) - - # thumb: resultant(6) + taxels(3) + index: resultant(6) + taxels(3) - data = ( - struct.pack(" Date: Fri, 3 Apr 2026 16:33:37 +0200 Subject: [PATCH 05/20] Remove tactile sensing UI to separate orca_ui repository Moves the web-based sensor visualization (Flask/SocketIO) out of orca_core per reviewer feedback on separation of concerns. The UI now lives in its own repo and depends on orca_core as an external package. --- pyproject.toml | 7 - scripts/tactile_sensing_ui/README.md | 26 - scripts/tactile_sensing_ui/static/script.js | 879 ------------------ scripts/tactile_sensing_ui/static/style.css | 628 ------------- scripts/tactile_sensing_ui/tactile_ui.py | 334 ------- .../tactile_sensing_ui/templates/index.html | 230 ----- 6 files changed, 2104 deletions(-) delete mode 100644 scripts/tactile_sensing_ui/README.md delete mode 100644 scripts/tactile_sensing_ui/static/script.js delete mode 100644 scripts/tactile_sensing_ui/static/style.css delete mode 100755 scripts/tactile_sensing_ui/tactile_ui.py delete mode 100644 scripts/tactile_sensing_ui/templates/index.html diff --git a/pyproject.toml b/pyproject.toml index 25a98e11..487b8a69 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -16,13 +16,6 @@ dependencies = [ "numpy (>=2.2.6,<3.0.0)", ] -[project.optional-dependencies] -sensing-ui = [ - "flask>=3.0.0,<4.0.0", - "flask-socketio>=5.6.0,<6.0.0", - "pyserial>=3.5,<4.0.0", -] - [dependency-groups] dev = [ "matplotlib>=3.10.1,<4.0.0", diff --git a/scripts/tactile_sensing_ui/README.md b/scripts/tactile_sensing_ui/README.md deleted file mode 100644 index ead2f4fe..00000000 --- a/scripts/tactile_sensing_ui/README.md +++ /dev/null @@ -1,26 +0,0 @@ -# ORCA Tactile Sensing UI - -Web-based interface for real-time tactile sensor visualization. - -## Usage - -```bash -python scripts/tactile_sensing_ui/tactile_ui.py -python scripts/tactile_sensing_ui/tactile_ui.py --config orca_core/models/v2/orcahand-touch/config.yaml -``` - -Then open your browser to `http://localhost:5001` - -## Features - -- **Connection Management**: Connect/disconnect to sensor devices -- **Sensor Status**: View which sensors are connected -- **Taxel Counts**: Display number of taxels for each sensor -- **Force Visualization**: Real-time force vectors displayed as arrows and numerical values -- **Taxel Visualization**: 2D taxel view with magnitude, direction, and arrow display modes -- **Zeroing**: Capture sensor baseline offsets -- **Auto Update**: Continuous monitoring of sensor data via WebSocket - -## Dependencies - -Install with: `pip install -e ".[sensing-ui]"` diff --git a/scripts/tactile_sensing_ui/static/script.js b/scripts/tactile_sensing_ui/static/script.js deleted file mode 100644 index 7be78654..00000000 --- a/scripts/tactile_sensing_ui/static/script.js +++ /dev/null @@ -1,879 +0,0 @@ -const MAX_FORCE_SCALE = 10; -const MIN_CIRCLE_RADIUS = 2; -const MAX_CIRCLE_RADIUS = 40; -const VISUALIZATION_RADIUS = 70; -const MAX_TAXEL_FORCE = 5; - -const socket = io(); - -// State -let currentMode = 'taxels'; -let taxelDisplayMode = 'direction'; // 'magnitude', 'direction', or 'arrows' -let arrowColorScheme = 'heat'; // 'heat', 'intensity', or 'orca' -let forceThreshold = 0.5; // default threshold in N -let arrowLengthMult = 1.0; -let arrowThicknessMult = 1.0; -let taxelCounts = { thumb: 127, index: 52, middle: 31, ring: 31, pinky: 31 }; -let taxelGridsInitialized = false; -let activeSensors = {}; -let taxelCoordinates = null; // Will be fetched from server - -socket.on('connect', () => { - console.log('WebSocket connected'); -}); - -socket.on('disconnect', () => { - console.log('WebSocket disconnected'); -}); - -socket.on('force_update', (forces) => { - updateForces(forces); - updateActiveSensorsFromForces(forces); -}); - -socket.on('taxel_update', (taxels) => { - updateTaxels(taxels); - updateActiveSensorsFromTaxels(taxels); -}); - -socket.on('combined_update', (data) => { - if (data.forces) { - updateForces(data.forces); - updateActiveSensorsFromForces(data.forces); - } - if (data.taxels) { - updateTaxels(data.taxels); - updateActiveSensorsFromTaxels(data.taxels); - } -}); - -socket.on('mode_changed', (data) => { - currentMode = data.mode; - updatePanelVisibility(); - document.getElementById('current-mode').textContent = getModeLabel(data.mode); - updateAutoDataTypeDisplay(data.mode); -}); - -socket.on('connection_status', (data) => { - if (data.connected) { - document.getElementById('connection-status').textContent = 'Connected'; - document.getElementById('connection-status').className = 'status-indicator connected'; - document.getElementById('connect-btn').disabled = true; - document.getElementById('disconnect-btn').disabled = false; - document.getElementById('refresh-btn').disabled = false; - if (data.mode) { - currentMode = data.mode; - document.getElementById('mode-select').value = data.mode; - } - updateUI(); - } else { - document.getElementById('connection-status').textContent = 'Disconnected'; - document.getElementById('connection-status').className = 'status-indicator disconnected'; - if (data.error) { - showError(data.error); - } - } -}); - -socket.on('error', (data) => { - showError(data.message); -}); - -socket.on('config_update', (data) => { - console.log('Sensor configuration changed:', data); - updateSensorConfig(data); -}); - -let currentView = '2d'; - -document.getElementById('connect-btn').addEventListener('click', connect); -document.getElementById('disconnect-btn').addEventListener('click', disconnect); -document.getElementById('refresh-btn').addEventListener('click', refresh); -document.getElementById('zero-btn').addEventListener('click', zeroSensors); -document.getElementById('reset-zero-btn').addEventListener('click', resetZero); -document.getElementById('scan-btn').addEventListener('click', scanPorts); -document.getElementById('mode-select').addEventListener('change', changeMode); -document.getElementById('magnitude-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('magnitude')); -document.getElementById('direction-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('direction')); -document.getElementById('arrows-mode-toggle').addEventListener('change', () => setTaxelDisplayMode('arrows')); -document.getElementById('color-scheme-select').addEventListener('change', (e) => { - arrowColorScheme = e.target.value; -}); -document.getElementById('arrow-length-slider').addEventListener('input', (e) => { - arrowLengthMult = parseFloat(e.target.value); - document.getElementById('arrow-length-value').textContent = arrowLengthMult.toFixed(1) + 'x'; - window.dispatchEvent(new CustomEvent('arrow-size-changed', { detail: { length: arrowLengthMult, thickness: arrowThicknessMult } })); -}); -document.getElementById('arrow-thickness-slider').addEventListener('input', (e) => { - arrowThicknessMult = parseFloat(e.target.value); - document.getElementById('arrow-thickness-value').textContent = arrowThicknessMult.toFixed(1) + 'x'; - window.dispatchEvent(new CustomEvent('arrow-size-changed', { detail: { length: arrowLengthMult, thickness: arrowThicknessMult } })); -}); -document.getElementById('threshold-toggle').addEventListener('change', (e) => { - const input = document.getElementById('threshold-input'); - input.disabled = !e.target.checked; - forceThreshold = e.target.checked ? parseFloat(input.value) || 0 : 0; - window.dispatchEvent(new CustomEvent('threshold-changed', { detail: { threshold: forceThreshold } })); -}); -document.getElementById('threshold-input').addEventListener('input', (e) => { - const toggle = document.getElementById('threshold-toggle'); - if (toggle.checked) { - forceThreshold = parseFloat(e.target.value) || 0; - window.dispatchEvent(new CustomEvent('threshold-changed', { detail: { threshold: forceThreshold } })); - } -}); - -// Auto-scan on page load -scanPorts(); - -function getModeLabel(mode) { - switch (mode) { - case 'resultant': return 'Resultant Force'; - case 'taxels': return 'Taxels Only'; - case 'combined': return 'Combined'; - default: return mode; - } -} - -async function scanPorts() { - const select = document.getElementById('port-select'); - const scanBtn = document.getElementById('scan-btn'); - scanBtn.disabled = true; - scanBtn.textContent = '...'; - try { - const response = await fetch('/api/ports'); - const ports = await response.json(); - const previousValue = select.value; - select.innerHTML = ''; - if (ports.length === 0) { - const opt = document.createElement('option'); - opt.value = '/dev/ttyACM0'; - opt.textContent = 'No ports found — /dev/ttyACM0'; - select.appendChild(opt); - } else { - ports.forEach(p => { - const opt = document.createElement('option'); - opt.value = p.device; - const label = p.is_sensor_adapter ? `${p.device} (Sensor Adapter)` : `${p.device} — ${p.description}`; - opt.textContent = label; - if (p.is_sensor_adapter) opt.style.fontWeight = '600'; - select.appendChild(opt); - }); - // Re-select previous value if still present, otherwise keep first (best match) - if ([...select.options].some(o => o.value === previousValue)) { - select.value = previousValue; - } - } - } catch (error) { - showError('Port scan failed: ' + error.message); - } finally { - scanBtn.disabled = false; - scanBtn.textContent = 'Scan'; - } -} - -async function fetchTaxelCoordinates() { - try { - const response = await fetch('/api/taxel_coordinates'); - taxelCoordinates = await response.json(); - return taxelCoordinates; - } catch (error) { - console.error('Failed to fetch taxel coordinates:', error); - return null; - } -} - -async function connect() { - const port = document.getElementById('port-select').value; - const mode = document.getElementById('mode-select').value; - try { - // Fetch coordinates before connecting - if (!taxelCoordinates) { - await fetchTaxelCoordinates(); - } - - const response = await fetch('/api/connect', { - method: 'POST', - headers: {'Content-Type': 'application/json'}, - body: JSON.stringify({port: port, mode: mode}) - }); - const data = await response.json(); - if (data.success) { - showError(null); - currentMode = data.mode; - if (data.config && data.config.num_taxels) { - taxelCounts = data.config.num_taxels; - } - document.getElementById('zero-btn').disabled = false; - initializeTaxelGrids(); - updatePanelVisibility(); - updateAutoDataTypeDisplay(data.mode); - updateUI(); - } else { - showError(data.message); - } - } catch (error) { - showError('Connection failed: ' + error.message); - } -} - -async function disconnect() { - try { - const response = await fetch('/api/disconnect', {method: 'POST'}); - const data = await response.json(); - if (data.success) { - showError(null); - document.getElementById('connection-status').textContent = 'Disconnected'; - document.getElementById('connection-status').className = 'status-indicator disconnected'; - document.getElementById('connect-btn').disabled = false; - document.getElementById('disconnect-btn').disabled = true; - document.getElementById('refresh-btn').disabled = true; - document.getElementById('zero-btn').disabled = true; - document.getElementById('reset-zero-btn').disabled = true; - document.getElementById('reset-zero-btn').style.display = 'none'; - document.getElementById('stream-status').style.display = 'none'; - } - } catch (error) { - showError('Disconnect failed: ' + error.message); - } -} - -async function zeroSensors() { - const btn = document.getElementById('zero-btn'); - btn.disabled = true; - btn.textContent = 'Zeroing...'; - try { - const response = await fetch('/api/zero', {method: 'POST'}); - const data = await response.json(); - if (data.success) { - showError(null); - document.getElementById('reset-zero-btn').style.display = ''; - document.getElementById('reset-zero-btn').disabled = false; - } else { - showError(data.message); - } - } catch (error) { - showError('Zero failed: ' + error.message); - } finally { - btn.disabled = false; - btn.textContent = 'Zero'; - } -} - -async function resetZero() { - try { - const response = await fetch('/api/clear_zero', {method: 'POST'}); - const data = await response.json(); - if (data.success) { - showError(null); - document.getElementById('reset-zero-btn').style.display = 'none'; - document.getElementById('reset-zero-btn').disabled = true; - } else { - showError(data.message); - } - } catch (error) { - showError('Reset zero failed: ' + error.message); - } -} - -async function changeMode() { - const mode = document.getElementById('mode-select').value; - try { - const response = await fetch('/api/mode', { - method: 'POST', - headers: {'Content-Type': 'application/json'}, - body: JSON.stringify({mode: mode}) - }); - const data = await response.json(); - if (data.success) { - currentMode = data.mode; - updatePanelVisibility(); - document.getElementById('current-mode').textContent = getModeLabel(data.mode); - updateAutoDataTypeDisplay(data.mode); - } else { - showError(data.message); - document.getElementById('mode-select').value = currentMode; - } - } catch (error) { - showError('Mode change failed: ' + error.message); - document.getElementById('mode-select').value = currentMode; - } -} - -function setTaxelDisplayMode(mode) { - taxelDisplayMode = mode; - - const magLegend = document.querySelector('.magnitude-legend'); - const dirLegend = document.querySelector('.direction-legend'); - const arrowsLegend = document.querySelector('.arrows-legend'); - - magLegend.style.display = mode === 'magnitude' ? 'inline-block' : 'none'; - dirLegend.style.display = mode === 'direction' ? 'inline-block' : 'none'; - arrowsLegend.style.display = mode === 'arrows' ? 'inline-block' : 'none'; - - // Clear arrows when switching away from arrows mode - if (mode !== 'arrows') { - clearAllArrows(); - } -} - -function clearAllArrows() { - document.querySelectorAll('.taxel-arrow').forEach(el => el.remove()); -} - -function updatePanelVisibility() { - const forcesPanel = document.getElementById('forces-panel'); - const taxelsPanel = document.getElementById('taxels-panel'); - - switch (currentMode) { - case 'resultant': - forcesPanel.style.display = 'block'; - taxelsPanel.style.display = 'none'; - break; - case 'taxels': - forcesPanel.style.display = 'none'; - taxelsPanel.style.display = 'block'; - break; - case 'combined': - forcesPanel.style.display = 'block'; - taxelsPanel.style.display = 'block'; - break; - } -} - -function initializeTaxelGrids() { - const container = document.getElementById('taxels-container'); - container.innerHTML = ''; - - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - - fingers.forEach(finger => { - const coords = taxelCoordinates ? taxelCoordinates[finger] : null; - const numTaxels = coords ? coords.length : (taxelCounts[finger] || 31); - - const fingerDiv = document.createElement('div'); - fingerDiv.className = 'taxel-finger'; - fingerDiv.dataset.finger = finger; - - const label = document.createElement('div'); - label.className = 'taxel-finger-label'; - label.textContent = finger.charAt(0).toUpperCase() + finger.slice(1); - fingerDiv.appendChild(label); - - if (coords && coords.length > 0) { - // Use coordinate-based SVG rendering - const svg = createCoordinateSVG(finger, coords); - fingerDiv.appendChild(svg); - } else { - // Fallback to simple grid if no coordinates - const grid = createFallbackGrid(finger, numTaxels); - fingerDiv.appendChild(grid); - } - - container.appendChild(fingerDiv); - }); - - taxelGridsInitialized = true; -} - -function createCoordinateSVG(finger, coords) { - // Calculate bounds - let minX = Infinity, maxX = -Infinity; - let minY = Infinity, maxY = -Infinity; - - coords.forEach(c => { - minX = Math.min(minX, c.x); - maxX = Math.max(maxX, c.x); - minY = Math.min(minY, c.y); - maxY = Math.max(maxY, c.y); - }); - - const dataWidth = maxX - minX; - const dataHeight = maxY - minY; - - // SVG dimensions - scale based on finger size - const padding = 8; - const taxelRadius = finger === 'thumb' ? 5 : 5; - const scale = finger === 'thumb' ? 7.5 : 7; - - const svgWidth = dataWidth * scale + padding * 2; - const svgHeight = dataHeight * scale + padding * 2; - - const svg = document.createElementNS('http://www.w3.org/2000/svg', 'svg'); - svg.setAttribute('class', 'taxel-svg'); - svg.setAttribute('viewBox', `0 0 ${svgWidth} ${svgHeight}`); - svg.setAttribute('width', svgWidth); - svg.setAttribute('height', svgHeight); - svg.dataset.finger = finger; - - // Add taxel circles - coords.forEach((coord, index) => { - const circle = document.createElementNS('http://www.w3.org/2000/svg', 'circle'); - - // Transform coordinates to SVG space - // X: left-to-right maps to SVG x - // Y: sensor Y (proximal-to-distal) maps to SVG y (top-to-bottom, inverted) - const svgX = (coord.x - minX) * scale + padding; - const svgY = svgHeight - ((coord.y - minY) * scale + padding); // Invert Y - - circle.setAttribute('cx', svgX); - circle.setAttribute('cy', svgY); - circle.setAttribute('r', taxelRadius); - circle.setAttribute('fill', '#1a1a1a'); - circle.setAttribute('stroke', '#2a2a2a'); - circle.setAttribute('stroke-width', '0.5'); - circle.setAttribute('class', 'taxel-circle'); - circle.setAttribute('id', `taxel-${finger}-${index}`); - circle.dataset.taxelIndex = index; - - svg.appendChild(circle); - }); - - return svg; -} - -function createFallbackGrid(finger, numTaxels) { - // Simple fallback grid when coordinates are not available - const grid = document.createElement('div'); - grid.className = 'taxel-grid'; - grid.dataset.finger = finger; - - const cols = finger === 'thumb' ? 11 : 6; - const rows = Math.ceil(numTaxels / cols); - - let taxelIndex = 0; - for (let row = 0; row < rows; row++) { - const rowDiv = document.createElement('div'); - rowDiv.className = 'taxel-row'; - - for (let col = 0; col < cols && taxelIndex < numTaxels; col++) { - const cell = document.createElement('div'); - cell.className = 'taxel-cell'; - cell.dataset.taxelIndex = taxelIndex; - cell.id = `taxel-${finger}-${taxelIndex}`; - rowDiv.appendChild(cell); - taxelIndex++; - } - - grid.appendChild(rowDiv); - } - - return grid; -} - -function updateTaxels(taxels) { - if (!taxelGridsInitialized) return; - - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - - fingers.forEach(finger => { - const fingerTaxels = taxels[finger]; - if (!fingerTaxels) return; - - fingerTaxels.forEach((taxelData, index) => { - const element = document.getElementById(`taxel-${finger}-${index}`); - if (!element) return; - - const [fx, fy, fz] = taxelData; - const magnitude = Math.sqrt(fx * fx + fy * fy + fz * fz); - - // Check if it's an SVG circle or a div cell - const isSVG = element.tagName.toLowerCase() === 'circle'; - - // If below threshold, reset to default and skip - if (forceThreshold > 0 && magnitude < forceThreshold) { - if (isSVG) { - element.setAttribute('fill', '#1a1a1a'); - } else { - element.style.backgroundColor = '#1a1a1a'; - } - return; - } - - if (taxelDisplayMode === 'arrows' && isSVG) { - // Arrows mode - show 3D direction with intensity coloring - updateTaxelArrow(element, fx, fy, fz, magnitude); - } else if (taxelDisplayMode === 'direction') { - // Direction-based coloring - const absX = Math.abs(fx); - const absY = Math.abs(fy); - - let color = '#1a1a1a'; - if (magnitude >= 0.1) { - const alpha = Math.min(magnitude / MAX_TAXEL_FORCE, 1); - const opacity = 0.3 + alpha * 0.7; - - if (absX > absY) { - if (fx > 0) { - color = `rgba(239, 68, 68, ${opacity})`; - } else { - color = `rgba(6, 182, 212, ${opacity})`; - } - } else { - if (fy > 0) { - color = `rgba(16, 185, 129, ${opacity})`; - } else { - color = `rgba(245, 158, 11, ${opacity})`; - } - } - } - - if (isSVG) { - element.setAttribute('fill', color); - } else { - element.style.backgroundColor = color; - } - } else { - // Magnitude-based coloring (grayscale) - const normalized = Math.min(magnitude / MAX_TAXEL_FORCE, 1); - const gray = Math.round(60 + normalized * 180); - const color = `rgb(${gray}, ${gray}, ${gray})`; - - if (isSVG) { - element.setAttribute('fill', color); - } else { - element.style.backgroundColor = color; - } - } - }); - }); -} - -function getArrowColor2D(normalized) { - switch (arrowColorScheme) { - case 'intensity': { - const l = Math.round(15 + normalized * 85); - return `hsl(0, 0%, ${l}%)`; - } - case 'orca': { - // ORCA palette gradient: #474f5e → #7f8ea2 → #bfc7d1 → #e5e7eb - const stops = [ - [71, 79, 94], // #474f5e - [127, 142, 162], // #7f8ea2 - [191, 199, 209], // #bfc7d1 - [229, 231, 235], // #e5e7eb - ]; - const scaled = normalized * (stops.length - 1); - const idx = Math.min(Math.floor(scaled), stops.length - 2); - const frac = scaled - idx; - const r = Math.round(stops[idx][0] + (stops[idx + 1][0] - stops[idx][0]) * frac); - const g = Math.round(stops[idx][1] + (stops[idx + 1][1] - stops[idx][1]) * frac); - const b = Math.round(stops[idx][2] + (stops[idx + 1][2] - stops[idx][2]) * frac); - return `rgb(${r}, ${g}, ${b})`; - } - default: { // 'heat' - const hue = (1 - normalized) * 240; - const saturation = 70 + normalized * 30; - const lightness = 55 - normalized * 10; - return `hsl(${hue}, ${saturation}%, ${lightness}%)`; - } - } -} - -function updateTaxelArrow(circleElement, fx, fy, fz, magnitude) { - const svg = circleElement.closest('svg'); - if (!svg) return; - - const cx = parseFloat(circleElement.getAttribute('cx')); - const cy = parseFloat(circleElement.getAttribute('cy')); - const taxelId = circleElement.id; - const arrowId = `arrow-${taxelId}`; - - // Remove existing arrow - const existingArrow = document.getElementById(arrowId); - if (existingArrow) existingArrow.remove(); - - // Reset circle to light gray background - circleElement.setAttribute('fill', '#0a0a0a'); - - // Don't draw arrow if force is too small or below threshold - if (magnitude < 0.1) return; - if (forceThreshold > 0 && magnitude < forceThreshold) return; - - // Calculate arrow properties - const normalized = Math.min(magnitude / MAX_TAXEL_FORCE, 1); - - // XY magnitude for arrow direction in plane - const xyMag = Math.sqrt(fx * fx + fy * fy); - - // Arrow length based on XY magnitude, with Z affecting it - const baseLength = (4 + normalized * 10) * arrowLengthMult; - const zFactor = 1 + Math.abs(fz) / MAX_TAXEL_FORCE * 0.5; - const arrowLength = baseLength * (xyMag > 0.1 ? 1 : 0.3) * zFactor; - - // Arrow direction (in SVG coordinates, Y is inverted) - let angle = 0; - if (xyMag > 0.1) { - angle = Math.atan2(-fy, fx); // Negative fy because SVG Y is down - } - - // End point of arrow - const endX = cx + Math.cos(angle) * arrowLength; - const endY = cy + Math.sin(angle) * arrowLength; - - // Color based on selected scheme - const color = getArrowColor2D(normalized); - - // Create arrow group - const arrowGroup = document.createElementNS('http://www.w3.org/2000/svg', 'g'); - arrowGroup.setAttribute('id', arrowId); - arrowGroup.setAttribute('class', 'taxel-arrow'); - - // Arrow line - const line = document.createElementNS('http://www.w3.org/2000/svg', 'line'); - line.setAttribute('x1', cx); - line.setAttribute('y1', cy); - line.setAttribute('x2', endX); - line.setAttribute('y2', endY); - line.setAttribute('stroke', color); - line.setAttribute('stroke-width', (2 + normalized * 2.5) * arrowThicknessMult); - line.setAttribute('stroke-linecap', 'round'); - arrowGroup.appendChild(line); - - // Arrowhead (triangle) - if (arrowLength > 4) { - const headLength = (3 + normalized * 3) * arrowThicknessMult; - const headAngle = 0.6; // radians, about 35 degrees - - const head1X = endX - Math.cos(angle - headAngle) * headLength; - const head1Y = endY - Math.sin(angle - headAngle) * headLength; - const head2X = endX - Math.cos(angle + headAngle) * headLength; - const head2Y = endY - Math.sin(angle + headAngle) * headLength; - - const arrowhead = document.createElementNS('http://www.w3.org/2000/svg', 'polygon'); - arrowhead.setAttribute('points', `${endX},${endY} ${head1X},${head1Y} ${head2X},${head2Y}`); - arrowhead.setAttribute('fill', color); - arrowGroup.appendChild(arrowhead); - } - - svg.appendChild(arrowGroup); -} - -async function refresh() { - try { - const response = await fetch('/api/refresh'); - const data = await response.json(); - if (data.connected) { - updateStatus(data); - if (data.forces) updateForces(data.forces); - if (data.mode) { - currentMode = data.mode; - document.getElementById('mode-select').value = data.mode; - updatePanelVisibility(); - } - } else { - showError(data.error || 'Not connected'); - } - } catch (error) { - showError('Refresh failed: ' + error.message); - } -} - -async function updateUI() { - try { - // Ensure we have coordinates - if (!taxelCoordinates) { - await fetchTaxelCoordinates(); - } - - const response = await fetch('/api/status'); - const data = await response.json(); - if (data.connected) { - document.getElementById('connection-status').textContent = 'Connected'; - document.getElementById('connection-status').className = 'status-indicator connected'; - document.getElementById('connect-btn').disabled = true; - document.getElementById('disconnect-btn').disabled = false; - document.getElementById('refresh-btn').disabled = false; - document.getElementById('stream-status').style.display = 'block'; - updateStatus(data); - - if (data.taxels) { - taxelCounts = data.taxels; - if (!taxelGridsInitialized) { - initializeTaxelGrids(); - } - } - - if (data.mode) { - currentMode = data.mode; - document.getElementById('mode-select').value = data.mode; - document.getElementById('current-mode').textContent = getModeLabel(data.mode); - updatePanelVisibility(); - } - - const forcesResponse = await fetch('/api/forces'); - const forces = await forcesResponse.json(); - updateForces(forces); - } else { - document.getElementById('connection-status').textContent = 'Disconnected'; - document.getElementById('connection-status').className = 'status-indicator disconnected'; - } - } catch (error) { - showError('Update failed: ' + error.message); - } -} - -function updateAutoDataTypeDisplay(mode) { - const resultant = mode === 'resultant' || mode === 'combined'; - const taxels = mode === 'taxels' || mode === 'combined'; - document.getElementById('auto-data-type').innerHTML = ` - Resultant Force: ${resultant ? '✓' : '✗'}
- Individual Taxels: ${taxels ? '✓' : '✗'} - `; -} - -function updateStatus(data) { - document.getElementById('hardware-version').textContent = data.hardware_version || '-'; - - if (data.mode) { - document.getElementById('current-mode').textContent = getModeLabel(data.mode); - updateAutoDataTypeDisplay(data.mode); - } - - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - fingers.forEach(finger => { - const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); - const status = card.querySelector('.sensor-status'); - const taxels = card.querySelector('.sensor-taxels span'); - - if (data.sensors && data.sensors[finger]) { - status.className = 'sensor-status connected'; - status.textContent = '●'; - if (data.taxels) { - taxels.textContent = data.taxels[finger] || 0; - } - } else { - status.className = 'sensor-status disconnected'; - status.textContent = '●'; - taxels.textContent = '-'; - } - }); -} - -function updateForces(forces) { - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - - fingers.forEach(finger => { - if (!forces[finger]) return; - - const [fx, fy, fz] = forces[finger]; - const magnitude = Math.sqrt(fx*fx + fy*fy + fz*fz); - - document.getElementById(`fx-${finger}`).textContent = fx.toFixed(1); - document.getElementById(`fy-${finger}`).textContent = fy.toFixed(1); - document.getElementById(`fz-${finger}`).textContent = fz.toFixed(1); - document.getElementById(`mag-${finger}`).textContent = magnitude.toFixed(1); - - const circle = document.getElementById(`force-circle-${finger}`); - if (circle) { - const centerX = 100; - const centerY = 100; - - const normalizedMagnitude = Math.min(magnitude / MAX_FORCE_SCALE, 1); - const radius = MIN_CIRCLE_RADIUS + (normalizedMagnitude * (MAX_CIRCLE_RADIUS - MIN_CIRCLE_RADIUS)); - - const angle = Math.atan2(fy, fx); - const distance = Math.min(normalizedMagnitude * VISUALIZATION_RADIUS, VISUALIZATION_RADIUS); - const circleX = centerX + distance * Math.cos(angle); - const circleY = centerY - distance * Math.sin(angle); - - circle.setAttribute('cx', circleX); - circle.setAttribute('cy', circleY); - circle.setAttribute('r', radius); - - const opacity = Math.min(0.3 + normalizedMagnitude * 0.7, 1); - const color = magnitude > 1 ? '#ef4444' : '#3b82f6'; - circle.setAttribute('fill', color); - circle.setAttribute('opacity', opacity); - } - }); -} - - -function showError(message) { - const panel = document.getElementById('error-panel'); - const msg = document.getElementById('error-message'); - if (message) { - msg.textContent = message; - panel.style.display = 'block'; - } else { - panel.style.display = 'none'; - } -} - -function updateActiveSensorsFromForces(forces) { - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - fingers.forEach(finger => { - if (forces[finger]) { - activeSensors[finger] = true; - updateSensorStatusDisplay(finger, true); - } - }); -} - -function updateActiveSensorsFromTaxels(taxels) { - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - fingers.forEach(finger => { - if (taxels[finger] && taxels[finger].length > 0) { - activeSensors[finger] = true; - updateSensorStatusDisplay(finger, true, taxels[finger].length); - } - }); -} - -function updateSensorStatusDisplay(finger, connected, taxelCount) { - const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); - if (!card) return; - - const status = card.querySelector('.sensor-status'); - if (connected) { - status.className = 'sensor-status connected'; - status.textContent = '●'; - } else { - status.className = 'sensor-status disconnected'; - status.textContent = '●'; - } - - if (taxelCount !== undefined) { - const taxelsSpan = card.querySelector('.sensor-taxels span'); - if (taxelsSpan) { - taxelsSpan.textContent = taxelCount; - } - } -} - -function updateSensorConfig(data) { - const fingers = ['thumb', 'index', 'middle', 'ring', 'pinky']; - - fingers.forEach(finger => { - const card = document.querySelector(`.sensor-card[data-finger="${finger}"]`); - if (!card) return; - - const status = card.querySelector('.sensor-status'); - const taxelsSpan = card.querySelector('.sensor-taxels span'); - - const isConnected = data.sensors && data.sensors[finger]; - - if (isConnected) { - status.className = 'sensor-status connected'; - status.textContent = '●'; - if (taxelsSpan && data.taxels) { - taxelsSpan.textContent = data.taxels[finger] || 0; - } - } else { - status.className = 'sensor-status disconnected'; - status.textContent = '●'; - if (taxelsSpan) { - taxelsSpan.textContent = '-'; - } - } - - // Update activeSensors tracking - activeSensors[finger] = isConnected; - }); - - // Update taxel counts for grid reinitialization if needed - if (data.taxels) { - taxelCounts = data.taxels; - } -} diff --git a/scripts/tactile_sensing_ui/static/style.css b/scripts/tactile_sensing_ui/static/style.css deleted file mode 100644 index 916a893a..00000000 --- a/scripts/tactile_sensing_ui/static/style.css +++ /dev/null @@ -1,628 +0,0 @@ -* { - margin: 0; - padding: 0; - box-sizing: border-box; -} - -body { - font-family: 'Space Mono', monospace; - background: #1a1c2a; - min-height: 100vh; - padding: 12px; - color: #dfe3e8; -} - -::selection { - background: rgba(34, 211, 238, 0.25); - color: #fff; -} - -.container { - max-width: 1600px; - margin: 0 auto; -} - -/* --- Header --- */ - -header { - background: rgba(255, 255, 255, 0.04); - backdrop-filter: blur(12px); - -webkit-backdrop-filter: blur(12px); - border-radius: 0; - padding: 12px 16px; - margin-bottom: 12px; - border: 1px solid rgba(255, 255, 255, 0.06); -} - -header h1 { - font-size: 16px; - font-weight: 700; - margin-bottom: 10px; - color: #fff; - letter-spacing: 0.5px; -} - -.connection-controls { - display: flex; - gap: 6px; - align-items: center; - flex-wrap: wrap; -} - -.port-selector { - display: flex; - gap: 4px; - align-items: center; -} - -#port-select { - padding: 5px 10px; - border: 1px solid rgba(255, 255, 255, 0.08); - border-radius: 0; - font-size: 11px; - font-family: 'Space Mono', monospace; - min-width: 180px; - max-width: 300px; - background: rgba(255, 255, 255, 0.04); - color: #dfe3e8; - outline: none; - transition: border-color 0.15s; -} - -#port-select:focus { - border-color: rgba(34, 211, 238, 0.4); -} - -#mode-select { - background: rgba(255, 255, 255, 0.04); - color: #dfe3e8; - border: 1px solid rgba(255, 255, 255, 0.08); - border-radius: 0; - padding: 5px 10px; - font-size: 11px; - font-family: 'Space Mono', monospace; - outline: none; - transition: border-color 0.15s; -} - -#mode-select:focus { - border-color: rgba(34, 211, 238, 0.4); -} - -/* --- Buttons --- */ - -.btn { - padding: 5px 10px; - border: 1px solid transparent; - border-radius: 0; - font-size: 11px; - font-weight: 600; - cursor: pointer; - transition: all 0.15s; - font-family: 'Space Mono', monospace; - letter-spacing: 0.3px; -} - -.btn:disabled { - opacity: 0.35; - cursor: not-allowed; -} - -.btn-primary { - background: rgba(34, 211, 238, 0.12); - color: #22d3ee; - border-color: rgba(34, 211, 238, 0.25); -} - -.btn-primary:hover:not(:disabled) { - background: rgba(34, 211, 238, 0.2); - border-color: rgba(34, 211, 238, 0.4); -} - -.btn-secondary { - background: rgba(255, 255, 255, 0.04); - color: #9faab9; - border-color: rgba(255, 255, 255, 0.08); -} - -.btn-secondary:hover:not(:disabled) { - background: rgba(255, 255, 255, 0.08); - color: #dfe3e8; -} - -.btn-info { - background: rgba(16, 185, 129, 0.12); - color: #34d399; - border-color: rgba(16, 185, 129, 0.25); -} - -.btn-info:hover:not(:disabled) { - background: rgba(16, 185, 129, 0.2); - border-color: rgba(16, 185, 129, 0.4); -} - -.btn-scan { - background: rgba(139, 92, 246, 0.12); - color: #a78bfa; - border-color: rgba(139, 92, 246, 0.25); - font-size: 10px; - padding: 5px 8px; -} - -.btn-scan:hover:not(:disabled) { - background: rgba(139, 92, 246, 0.2); - border-color: rgba(139, 92, 246, 0.4); -} - -.status-badge { - padding: 4px 10px; - background: rgba(34, 211, 238, 0.08); - color: #22d3ee; - border: 1px solid rgba(34, 211, 238, 0.15); - border-radius: 0; - font-size: 10px; - font-weight: 600; - display: none; - letter-spacing: 0.3px; -} - -/* --- Status Panel --- */ - -.status-panel { - display: grid; - grid-template-columns: repeat(auto-fit, minmax(180px, 1fr)); - gap: 8px; - margin-bottom: 12px; -} - -.status-card { - background: rgba(255, 255, 255, 0.03); - backdrop-filter: blur(8px); - -webkit-backdrop-filter: blur(8px); - border-radius: 0; - padding: 10px 12px; - border: 1px solid rgba(255, 255, 255, 0.06); -} - -.status-card h3 { - font-size: 10px; - color: #7f8ea2; - margin-bottom: 5px; - text-transform: uppercase; - letter-spacing: 0.8px; -} - -.status-card div:not(h3) { - font-size: 12px; - color: #dfe3e8; -} - -.status-indicator { - font-size: 12px; - font-weight: 600; - padding: 3px 8px; - border-radius: 0; - display: inline-block; -} - -.status-indicator.connected { - background: rgba(16, 185, 129, 0.12); - color: #34d399; - border: 1px solid rgba(16, 185, 129, 0.2); -} - -.status-indicator.disconnected { - background: rgba(200, 100, 100, 0.1); - color: #d4878a; - border: 1px solid rgba(200, 100, 100, 0.15); -} - -/* --- Sensors Panel --- */ - -.sensors-panel, .forces-panel { - background: rgba(255, 255, 255, 0.03); - backdrop-filter: blur(8px); - -webkit-backdrop-filter: blur(8px); - border-radius: 0; - padding: 12px; - margin-bottom: 12px; - border: 1px solid rgba(255, 255, 255, 0.06); -} - -.sensors-panel h2, .forces-panel h2 { - font-size: 13px; - font-weight: 700; - margin-bottom: 10px; - color: #fff; - letter-spacing: 0.3px; -} - -.sensors-grid { - display: grid; - grid-template-columns: repeat(auto-fit, minmax(120px, 1fr)); - gap: 6px; -} - -.sensor-card { - background: rgba(255, 255, 255, 0.03); - border: 1px solid rgba(255, 255, 255, 0.06); - border-radius: 0; - padding: 8px; - text-align: center; - transition: all 0.15s; -} - -.sensor-card:hover { - border-color: rgba(255, 255, 255, 0.12); - background: rgba(255, 255, 255, 0.05); -} - -.sensor-name { - font-size: 11px; - font-weight: 600; - margin-bottom: 4px; - color: #dfe3e8; - letter-spacing: 0.3px; -} - -.sensor-status { - font-size: 18px; - margin-bottom: 4px; - line-height: 1; -} - -.sensor-status.connected { - color: #34d399; -} - -.sensor-status.disconnected { - color: #d4878a; -} - -.sensor-taxels { - font-size: 10px; - color: #7f8ea2; -} - -.sensor-taxels span { - font-weight: 600; - color: #dfe3e8; -} - -/* --- Forces Panel --- */ - -.forces-container { - display: grid; - grid-template-columns: repeat(auto-fit, minmax(260px, 1fr)); - gap: 10px; -} - -.force-visualization { - background: rgba(255, 255, 255, 0.02); - border: 1px solid rgba(255, 255, 255, 0.06); - border-radius: 0; - padding: 16px; - text-align: center; -} - -.force-label { - font-size: 14px; - font-weight: 700; - margin-bottom: 12px; - color: #fff; - letter-spacing: 0.3px; -} - -.force-arrow { - width: 100%; - height: 200px; - margin: 12px 0; - position: relative; -} - -.force-arrow circle { - transition: cx 0.05s ease-out, cy 0.05s ease-out, r 0.05s ease-out, opacity 0.05s ease-out; -} - -.force-values { - display: grid; - grid-template-columns: repeat(2, 1fr); - gap: 6px; - font-size: 12px; - color: #7f8ea2; -} - -.force-values span { - font-weight: 600; - color: #dfe3e8; -} - -.force-magnitude { - grid-column: 1 / -1; - font-size: 13px; - font-weight: 600; - color: #22d3ee; - margin-top: 6px; - padding-top: 6px; - border-top: 1px solid rgba(255, 255, 255, 0.06); -} - -/* --- Taxels Panel --- */ - -.taxels-panel { - background: rgba(255, 255, 255, 0.03); - backdrop-filter: blur(8px); - -webkit-backdrop-filter: blur(8px); - border-radius: 0; - padding: 12px; - margin-bottom: 12px; - border: 1px solid rgba(255, 255, 255, 0.06); -} - -.taxels-header { - display: flex; - justify-content: space-between; - align-items: center; - margin-bottom: 10px; - flex-wrap: wrap; - gap: 8px; -} - -.taxels-header h2 { - font-size: 13px; - font-weight: 700; - color: #fff; - margin: 0; - letter-spacing: 0.3px; -} - -/* --- Taxel Controls Toolbar --- */ - -.taxel-controls { - display: flex; - align-items: center; - gap: 0; - flex-wrap: wrap; - background: rgba(255, 255, 255, 0.03); - border: 1px solid rgba(255, 255, 255, 0.06); - padding: 2px; -} - -.toggle-label { - display: flex; - align-items: center; - gap: 4px; - font-size: 10px; - cursor: pointer; - padding: 5px 10px; - background: transparent; - color: #7f8ea2; - border: none; - border-right: 1px solid rgba(255, 255, 255, 0.06); - border-radius: 0; - transition: all 0.15s; -} - -.toggle-label:last-child { - border-right: none; -} - -.toggle-label:hover { - background: rgba(255, 255, 255, 0.04); - color: #9faab9; -} - -.toggle-label:has(input:checked) { - background: rgba(34, 211, 238, 0.1); - color: #22d3ee; -} - -.toggle-label input[type="checkbox"], -.toggle-label input[type="radio"] { - display: none; -} - -.toggle-label input[type="checkbox"] { - display: inline-block; - width: 12px; - height: 12px; - cursor: pointer; - margin: 0; - accent-color: #22d3ee; -} - -.color-legend { - font-size: 9px; - color: #5f718b; - padding: 5px 10px; - border-right: none; -} - -.magnitude-legend, -.direction-legend, -.arrows-legend { - display: none; -} - -.direction-legend { - display: inline-flex; - gap: 4px; - align-items: center; -} - -.direction-legend span { - padding: 2px 6px; - margin-right: 0; - font-weight: 600; - font-size: 9px; - border: 1px solid; -} - -.dir-right { background: rgba(200, 100, 100, 0.1); color: #d4878a; border-color: rgba(200, 100, 100, 0.2) !important; } -.dir-left { background: rgba(6, 182, 212, 0.12); color: #22d3ee; border-color: rgba(6, 182, 212, 0.2) !important; } -.dir-up { background: rgba(16, 185, 129, 0.12); color: #34d399; border-color: rgba(16, 185, 129, 0.2) !important; } -.dir-down { background: rgba(200, 160, 80, 0.12); color: #c8a870; border-color: rgba(200, 160, 80, 0.2) !important; } - -/* --- Row Labels --- */ - -.viz-row-label { - font-size: 10px; - font-weight: 700; - color: #5f718b; - text-transform: uppercase; - letter-spacing: 1px; - padding: 6px 0 4px 0; -} - -/* --- Taxel Grids (2D) --- */ - -.taxels-container { - display: grid; - grid-template-columns: repeat(5, 1fr); - gap: 10px; -} - -.taxel-finger { - background: #000000; - border: 1px solid rgba(255, 255, 255, 0.06); - border-radius: 0; - padding: 10px; - text-align: center; - aspect-ratio: 1 / 1; - display: flex; - flex-direction: column; - align-items: center; - justify-content: center; -} - -.taxel-finger-label { - font-size: 12px; - font-weight: 600; - margin-bottom: 8px; - color: #dfe3e8; - letter-spacing: 0.3px; -} - -.taxel-grid { - display: flex; - flex-direction: column; - align-items: center; - gap: 1px; -} - -.taxel-row { - display: flex; - gap: 1px; - justify-content: center; -} - -.taxel-cell { - width: 10px; - height: 10px; - border-radius: 0; - background: #1a1a1a; - transition: background-color 0.05s ease-out; -} - -.taxel-cell.inactive { - visibility: hidden; -} - -.taxel-cell.dir-right { background: #c47070; } -.taxel-cell.dir-left { background: #06b6d4; } -.taxel-cell.dir-up { background: #10b981; } -.taxel-cell.dir-down { background: #c8a050; } - -/* --- SVG Taxels --- */ - -.taxel-svg { - display: block; - margin: 0 auto; -} - -.taxel-circle { - transition: fill 0.05s ease-out; -} - -.taxel-arrow { - pointer-events: none; -} - -.taxel-arrow line, -.taxel-arrow polygon { - transition: stroke 0.05s ease-out, fill 0.05s ease-out; -} - -.view-separator { - display: none; -} - -/* --- Slider Controls --- */ - -.slider-control input[type="range"] { - width: 56px; - height: 3px; - cursor: pointer; - accent-color: #22d3ee; -} - -.slider-control span:last-child { - min-width: 26px; - font-size: 9px; - text-align: right; - color: #7f8ea2; -} - -#color-scheme-select { - background: rgba(255, 255, 255, 0.04); - color: #9faab9; - border: 1px solid rgba(255, 255, 255, 0.08); - border-radius: 0; - font-size: 9px; - font-family: 'Space Mono', monospace; - padding: 2px 6px; - cursor: pointer; - outline: none; -} - -#color-scheme-select:focus { - border-color: rgba(34, 211, 238, 0.3); -} - -#threshold-input { - width: 44px; - background: rgba(255, 255, 255, 0.04); - color: #9faab9; - border: 1px solid rgba(255, 255, 255, 0.08); - border-radius: 0; - font-size: 9px; - font-family: 'Space Mono', monospace; - padding: 2px 6px; - text-align: center; - outline: none; -} - -#threshold-input:focus { - border-color: rgba(34, 211, 238, 0.3); -} - -#threshold-input:disabled { - opacity: 0.3; - cursor: not-allowed; -} - -/* --- Error Panel --- */ - -.error-panel { - background: rgba(200, 100, 100, 0.08); - border: 1px solid rgba(200, 100, 100, 0.2); - border-radius: 0; - padding: 10px; - margin-top: 12px; -} - -.error-message { - color: #d4878a; - font-weight: 600; - font-size: 11px; -} diff --git a/scripts/tactile_sensing_ui/tactile_ui.py b/scripts/tactile_sensing_ui/tactile_ui.py deleted file mode 100755 index eac059a2..00000000 --- a/scripts/tactile_sensing_ui/tactile_ui.py +++ /dev/null @@ -1,334 +0,0 @@ -#!/usr/bin/env python3 -"""Web-based testing UI for ORCA Sensor Client""" - -from flask import Flask, render_template, jsonify, request -from flask_socketio import SocketIO, emit -import sys -import os -sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) - -from orca_core.hardware.sensor_client import SensorClient -from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates -from orca_core.utils.utils import read_yaml, update_yaml -import argparse -import yaml -import serial.tools.list_ports -import threading -import time - -SENSOR_ADAPTER_VID = 0x28E9 -SENSOR_ADAPTER_PID = 0x018A - -app = Flask(__name__) -app.config['SECRET_KEY'] = 'orca_sensor_secret' -socketio = SocketIO(app, cors_allowed_origins="*", async_mode='threading') - -sensor_client = None -stream_thread = None -stream_thread_running = False -current_mode = 'resultant' # 'resultant', 'taxels', or 'combined' -finger_to_sensor_id_config = None # Loaded from --config if provided -config_dir = None # Set from --config arg directory, for calibration.yaml access - -def get_sensor_client(): - global sensor_client - if sensor_client is None: - port = request.args.get('port', '/dev/ttyACM0') - sensor_client = SensorClient(port=port) - return sensor_client - -def stream_update_loop(): - """Background thread that reads from auto-stream and emits via websocket.""" - global stream_thread_running, sensor_client, current_mode - - while stream_thread_running: - try: - if sensor_client and sensor_client.is_connected: - if current_mode == 'resultant': - forces, ts = sensor_client.get_auto_latest() - if forces is not None: - socketio.emit('force_update', forces) - elif current_mode == 'taxels': - taxels, ts = sensor_client.get_auto_latest_taxels() - if taxels is not None: - socketio.emit('taxel_update', taxels) - elif current_mode == 'combined': - forces, taxels, ts = sensor_client.get_auto_latest_all() - if forces is not None or taxels is not None: - socketio.emit('combined_update', { - 'forces': forces, - 'taxels': taxels - }) - time.sleep(0.01) # ~100Hz update rate - except Exception as e: - socketio.emit('error', {'message': str(e)}) - time.sleep(0.1) - -def start_stream(mode): - """Start auto-stream with specified mode.""" - global sensor_client, stream_thread, stream_thread_running, current_mode - - # Stop existing stream - stop_stream() - - current_mode = mode - - # Configure and start auto-stream - if mode == 'resultant': - sensor_client.start_auto_stream(resultant=True, taxels=False) - elif mode == 'taxels': - sensor_client.start_auto_stream(resultant=False, taxels=True) - elif mode == 'combined': - sensor_client.start_auto_stream(resultant=True, taxels=True) - - # Start websocket emission thread - stream_thread_running = True - stream_thread = threading.Thread(target=stream_update_loop, daemon=True) - stream_thread.start() - -def stop_stream(): - """Stop auto-stream and emission thread.""" - global sensor_client, stream_thread, stream_thread_running - - stream_thread_running = False - if stream_thread: - stream_thread.join(timeout=1) - stream_thread = None - - if sensor_client and sensor_client.is_connected: - try: - sensor_client.stop_auto_stream() - except Exception: - pass - -@app.route('/') -def index(): - return render_template('index.html') - -@app.route('/api/ports') -def list_ports(): - ports = serial.tools.list_ports.comports() - result = [] - for p in ports: - is_sensor = (p.vid == SENSOR_ADAPTER_VID and p.pid == SENSOR_ADAPTER_PID) - if p.vid is not None: - result.append({ - 'device': p.device, - 'description': p.description, - 'is_sensor_adapter': is_sensor, - }) - result.sort(key=lambda x: (not x['is_sensor_adapter'], x['device'])) - return jsonify(result) - -@app.route('/api/connect', methods=['POST']) -def connect(): - try: - data = request.json - port = data.get('port', '/dev/ttyACM0') - mode = data.get('mode', 'resultant') - global sensor_client, current_mode - - if sensor_client and sensor_client.is_connected: - stop_stream() - sensor_client.disconnect() - - sensor_client = SensorClient(port=port, finger_to_sensor_id=finger_to_sensor_id_config) - sensor_client.connect() - - # Load saved sensor offsets if config was provided - if config_dir: - calib_path = os.path.join(config_dir, 'calibration.yaml') - calib_data = read_yaml(calib_path) - if calib_data and 'sensor_offsets' in calib_data: - sensor_client.set_taxel_offsets(calib_data['sensor_offsets']) - - # Start streaming with requested mode - start_stream(mode) - - # Get configuration for response - config = sensor_client.get_sensor_configuration() - - socketio.emit('connection_status', {'connected': True, 'mode': mode}) - return jsonify({ - 'success': True, - 'message': f'Connected to {port}', - 'mode': mode, - 'config': { - 'active_sensors': config.active_sensors if config else [], - 'num_taxels': config.num_taxels if config else {} - } - }) - except Exception as e: - socketio.emit('connection_status', {'connected': False, 'error': str(e)}) - return jsonify({'success': False, 'message': str(e)}), 400 - -@app.route('/api/disconnect', methods=['POST']) -def disconnect(): - try: - global sensor_client - stop_stream() - if sensor_client and sensor_client.is_connected: - sensor_client.disconnect() - socketio.emit('connection_status', {'connected': False}) - return jsonify({'success': True, 'message': 'Disconnected'}) - except Exception as e: - return jsonify({'success': False, 'message': str(e)}), 400 - -@app.route('/api/mode', methods=['POST']) -def set_mode(): - """Change the streaming mode.""" - try: - global sensor_client, current_mode - data = request.json - mode = data.get('mode', 'resultant') - - if mode not in ('resultant', 'taxels', 'combined'): - return jsonify({'success': False, 'message': f'Invalid mode: {mode}'}), 400 - - if not sensor_client or not sensor_client.is_connected: - return jsonify({'success': False, 'message': 'Not connected'}), 400 - - start_stream(mode) - socketio.emit('mode_changed', {'mode': mode}) - return jsonify({'success': True, 'mode': mode}) - except Exception as e: - return jsonify({'success': False, 'message': str(e)}), 400 - -@app.route('/api/zero', methods=['POST']) -def zero(): - """Capture current sensor readings as zero baseline.""" - try: - global sensor_client - if not sensor_client or not sensor_client.is_connected: - return jsonify({'success': False, 'message': 'Not connected'}), 400 - - offsets = sensor_client.capture_taxel_offsets(num_samples=100) - - if config_dir: - calib_path = os.path.join(config_dir, 'calibration.yaml') - update_yaml(calib_path, 'sensor_offsets', offsets) - - return jsonify({'success': True, 'message': 'Sensor offsets captured and applied'}) - except Exception as e: - return jsonify({'success': False, 'message': str(e)}), 400 - -@app.route('/api/clear_zero', methods=['POST']) -def clear_zero(): - """Clear sensor zeroing offsets.""" - try: - global sensor_client - if not sensor_client or not sensor_client.is_connected: - return jsonify({'success': False, 'message': 'Not connected'}), 400 - - sensor_client.clear_taxel_offsets() - return jsonify({'success': True, 'message': 'Sensor offsets cleared'}) - except Exception as e: - return jsonify({'success': False, 'message': str(e)}), 400 - -@app.route('/api/status') -def status(): - client = get_sensor_client() - if not client.is_connected: - return jsonify({'connected': False}) - - out = {'connected': True, 'mode': current_mode} - errors = [] - - try: - out['sensors'] = client.read_connected_sensors() - except Exception as e: - errors.append(f"read_connected_sensors: {e}") - - try: - out['taxels'] = client.read_num_taxels() - except Exception as e: - errors.append(f"read_num_taxels: {e}") - - try: - out['auto_data_type'] = client.read_auto_data_type() - except Exception as e: - errors.append(f"read_auto_data_type: {e}") - - # Get stream stats - try: - stats = client.get_auto_stats() - out['stream_stats'] = { - 'frames_ok': stats.frames_ok, - 'parse_ok': stats.parse_ok, - 'parse_errors': stats.parse_errors - } - except Exception: - pass - - out['status_ok'] = (len(errors) == 0) - if errors: - out['errors'] = errors - - return jsonify(out) - - -@app.route('/api/forces') -def forces(): - try: - client = get_sensor_client() - if not client.is_connected: - return jsonify({'error': 'Not connected'}), 400 - forces = client.read_resultant_force() - return jsonify(forces) - except Exception as e: - return jsonify({'error': str(e)}), 400 - -@app.route('/api/taxel_coordinates') -def taxel_coordinates(): - """Return taxel coordinates for all fingers.""" - return jsonify(get_all_coordinates()) - - -@app.route('/api/refresh') -def refresh(): - try: - client = get_sensor_client() - if not client.is_connected: - return jsonify({'connected': False}) - - connected = client.read_connected_sensors() - taxels = client.read_num_taxels() - auto_data = client.read_auto_data_type() - forces = client.read_resultant_force() - - return jsonify({ - 'connected': True, - 'sensors': connected, - 'taxels': taxels, - 'auto_data_type': auto_data, - 'forces': forces, - 'mode': current_mode - }) - except Exception as e: - return jsonify({'connected': False, 'error': str(e)}), 400 - -@socketio.on('connect') -def handle_connect(): - emit('connected', {'data': 'Connected to WebSocket'}) - -@socketio.on('disconnect') -def handle_disconnect(): - pass - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='ORCA Sensor Testing UI') - parser.add_argument('--config', type=str, default=None, - help='Path to hand config YAML (for sensor wiring mapping)') - args = parser.parse_args() - - if args.config: - config_dir = os.path.dirname(os.path.abspath(args.config)) - with open(args.config) as f: - config_data = yaml.safe_load(f) - sensors_cfg = config_data.get('sensors', {}) - mapping = sensors_cfg.get('finger_to_sensor_id') - if mapping: - finger_to_sensor_id_config = mapping - print(f"Loaded sensor mapping from {args.config}: {finger_to_sensor_id_config}") - - socketio.run(app, host='0.0.0.0', port=5001, debug=True, allow_unsafe_werkzeug=True) diff --git a/scripts/tactile_sensing_ui/templates/index.html b/scripts/tactile_sensing_ui/templates/index.html deleted file mode 100644 index d77cff3f..00000000 --- a/scripts/tactile_sensing_ui/templates/index.html +++ /dev/null @@ -1,230 +0,0 @@ - - - - - - ORCA Sensor Testing UI - - - - - - - -
-
-

ORCA Sensor Testing UI

-
-
- - -
- - - - - - -
Streaming: 100Hz
-
-
- -
-
-

Connection Status

-
Disconnected
-
-
-

Hardware Version

-
-
-
-
-

Stream Mode

-
-
-
-
-

Auto Data Type

-
-
-
-
- -
-

Sensor Status

-
-
-
Thumb
-
*
-
Taxels: -
-
-
-
Index
-
*
-
Taxels: -
-
-
-
Middle
-
*
-
Taxels: -
-
-
-
Ring
-
*
-
Taxels: -
-
-
-
Pinky
-
*
-
Taxels: -
-
-
-
- - - - - -
-
-

Taxel Visualization

-
- - - - | - - | - - - | - -
- - - Right - Up - Left - Down - - -
-
-
-
- -
-
- - -
- - - - From 4dcb569036428d8aa8c74c5e3d5e180c9b631478 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Fri, 3 Apr 2026 16:46:20 +0200 Subject: [PATCH 06/20] Add MockOrcaHandTouch and modernize type annotations Add MockOrcaHandTouch class backed by mock motor and sensor clients for testing hand+sensor workflows without hardware. Extract _create_sensor_client factory method from OrcaHandTouch to mirror the existing _create_motor_client pattern. Update sensor client type annotations to use PEP 604 union syntax. --- orca_core/hardware/mock_sensor_client.py | 12 ++--- orca_core/hardware/sensing/types.py | 58 ++++++++++++++++++++++++ orca_core/hardware/sensor_client.py | 25 +++++----- orca_core/hardware_hand.py | 40 +++++++++++++--- 4 files changed, 111 insertions(+), 24 deletions(-) create mode 100644 orca_core/hardware/sensing/types.py diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 23e6bbe4..2ef53af3 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -9,7 +9,7 @@ from __future__ import annotations -from typing import Callable, Optional +from collections.abc import Callable import time import logging @@ -54,11 +54,11 @@ def __init__( self, port: str = "mock", baudrate: int = DEFAULT_SENSOR_BAUDRATE, - connected_sensors: Optional[list[str]] = None, - finger_to_sensor_id: Optional[dict[str, int]] = None, - resultant_provider: Optional[ResultantProvider] = None, - taxel_provider: Optional[TaxelProvider] = None, - auto_rate_hz: Optional[float] = None, + connected_sensors: list[str] | None = None, + finger_to_sensor_id: dict[str, int] | None = None, + resultant_provider: ResultantProvider | None = None, + taxel_provider: TaxelProvider | None = None, + auto_rate_hz: float | None = None, ): super().__init__(port=port, baudrate=baudrate, finger_to_sensor_id=finger_to_sensor_id) diff --git a/orca_core/hardware/sensing/types.py b/orca_core/hardware/sensing/types.py new file mode 100644 index 00000000..7ca0d26f --- /dev/null +++ b/orca_core/hardware/sensing/types.py @@ -0,0 +1,58 @@ +"""Typed containers for tactile sensor readings.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@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: str) -> list[float]: + return self.forces[finger] + + def __contains__(self, finger: str) -> 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: str) -> list[list[float]]: + return self.taxels[finger] + + def __contains__(self, finger: str) -> bool: + return finger in self.taxels + + @property + def fingers(self) -> list[str]: + return list(self.taxels.keys()) + + def as_array(self, finger: str) -> np.ndarray: + """Return an ``(n_taxels, 3)`` array for *finger*.""" + return np.array(self.taxels[finger]) diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index 2d22db61..46b8e7b7 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -5,8 +5,9 @@ # 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 + from dataclasses import dataclass, field -from typing import Optional import serial import threading import time @@ -137,12 +138,12 @@ class SensorClient: def __init__(self, port: str = DEFAULT_SENSOR_PORT, baudrate: int = DEFAULT_SENSOR_BAUDRATE, - finger_to_sensor_id: Optional[dict[str, int]] = None): + finger_to_sensor_id: dict[str, int] | None = None): self.port = port self.baudrate = baudrate self._connected = False - self._serial_connection: Optional[serial.Serial] = None + self._serial_connection: serial.Serial | None = None # Finger-to-sensor-id mapping (configurable wiring) if finger_to_sensor_id is None: @@ -164,10 +165,10 @@ def __init__(self, self._sensor_id_to_finger = {v: k for k, v in self._finger_to_sensor_id.items()} # Sensor configuration (dynamic, adapts to connected sensors) - self._sensor_config: Optional[SensorConfiguration] = None + self._sensor_config: SensorConfiguration | None = None self._last_reconfigure_time: float = 0.0 # Rate limiting for reconfiguration - self._auto_thread: Optional[threading.Thread] = None + self._auto_thread: threading.Thread | None = None self._auto_running = threading.Event() # Thread-safe flag for auto stream self._auto_lock = threading.Lock() self._auto_latest = None # parsed resultant forces dict @@ -179,8 +180,8 @@ def __init__(self, self._last_frame_debug_print: float = 0.0 # Per-taxel zeroing offsets - self._taxel_offsets: Optional[dict] = None # {finger: [[fx, fy, fz], ...], ...} - self._resultant_offsets: Optional[dict] = None # {finger: [fx, fy, fz], ...} + self._taxel_offsets: dict | None = None # {finger: [[fx, fy, fz], ...], ...} + self._resultant_offsets: dict | None = None # {finger: [fx, fy, fz], ...} @property def is_connected(self) -> bool: @@ -496,7 +497,7 @@ def read_resultant_force(self) -> dict[str, list[float]]: self._apply_resultant_offsets(result) return result - def get_sensor_configuration(self) -> Optional[SensorConfiguration]: + def get_sensor_configuration(self) -> SensorConfiguration | None: """Get the current sensor configuration snapshot. Returns: @@ -772,7 +773,7 @@ def clear_taxel_offsets(self) -> None: self._taxel_offsets = None self._resultant_offsets = None - def get_taxel_offsets(self) -> Optional[dict]: + def get_taxel_offsets(self) -> dict | None: """Return current per-taxel offsets (for saving to YAML).""" return self._taxel_offsets @@ -862,8 +863,8 @@ def _apply_resultant_offsets(self, forces: dict) -> None: def _apply_stream_offsets( self, - parsed_resultant: Optional[dict], - parsed_taxels: Optional[dict], + parsed_resultant: dict | None, + parsed_taxels: dict | None, ) -> None: """Apply zeroing offsets to parsed auto-stream data in-place. @@ -942,7 +943,7 @@ def _acquire_frame( parse_resultant: bool, parse_taxels: bool, min_sensors: int, - ) -> tuple[Optional[dict], Optional[dict]]: + ) -> tuple[dict | None, dict | None]: """Acquire and return the next parsed (resultant, taxels) frame. Reads one auto-stream frame from serial, validates LRC, handles payload diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 588c9012..44bc0432 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1169,18 +1169,21 @@ def __init__( ) self._sensor_client = None - def connect(self) -> tuple[bool, str]: - success, msg = super().connect() - if not success: - return success, msg - + def _create_sensor_client(self): from .hardware.sensor_client import SensorClient - self._sensor_client = SensorClient( + return SensorClient( port=self.config.sensor_port, baudrate=self.config.sensor_baudrate, finger_to_sensor_id=self.config.finger_to_sensor_id, ) + + def connect(self) -> tuple[bool, str]: + success, msg = super().connect() + if not success: + return success, msg + + self._sensor_client = self._create_sensor_client() try: self._sensor_client.connect() except Exception as e: @@ -1244,3 +1247,28 @@ def _create_motor_client(self) -> MotorClient: return MockDynamixelClient( self.config.motor_ids, self.config.port, self.config.baudrate ) + + +class MockOrcaHandTouch(OrcaHandTouch): + """Drop-in :class:`OrcaHandTouch` backed by mock motor and sensor clients, + for testing and prototyping. + + All methods behave identically to :class:`OrcaHandTouch` but no serial + ports are opened and both motor and sensor state are simulated in memory. + """ + + 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_sensor_client(self): + from .hardware.mock_sensor_client import MockSensorClient + + return MockSensorClient( + port="mock", + baudrate=self.config.sensor_baudrate, + finger_to_sensor_id=self.config.finger_to_sensor_id, + ) From 701801170efa4870d267ccf6a6206c022232d8ee Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 7 Apr 2026 09:59:15 +0200 Subject: [PATCH 07/20] Address review feedback on tactile sensing API - Clarify get_tactile_forces/get_tactile_taxels docstrings to show string-keyed dict access pattern and list valid finger keys - Remove unused taxel_coordinates module: 3D coordinate data is only consumed by the visualization UI, which now lives in orca_ui. Taxel counts remain in constants.py where the mock client uses them. --- .../models/touch-sensor-finger/config.yaml | 265 ------------------ .../models/touch-sensor-pinky/config.yaml | 157 ----------- .../models/touch-sensor-thumb/config.yaml | 157 ----------- .../hardware/sensing/taxel_coordinates.py | 72 ----- orca_core/hardware_hand.py | 35 ++- tests/test_tactile_sensor.py | 2 +- tests/test_taxel_coordinates.py | 22 -- 7 files changed, 28 insertions(+), 682 deletions(-) delete mode 100644 orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml delete mode 100644 orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml delete mode 100644 orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml delete mode 100644 orca_core/hardware/sensing/taxel_coordinates.py delete mode 100644 tests/test_taxel_coordinates.py diff --git a/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml deleted file mode 100644 index c15a42cb..00000000 --- a/orca_core/hardware/sensing/models/touch-sensor-finger/config.yaml +++ /dev/null @@ -1,265 +0,0 @@ -name: touch-sensor-finger -description: ORCA Fingertip - Finger -num_taxels: 87 -coordinates: -- x: 4.03569563 - y: 28.08244307 - z: 3.72577291 -- x: 6.62767274 - y: 24.49442143 - z: 2.9712037 -- x: 6.53635781 - y: 24.33135486 - z: 5.83911297 -- x: 4.09826233 - y: 27.00584015 - z: 7.05315498 -- x: 5.72524942 - y: 23.13913822 - z: 8.34520194 -- x: 1.215e-05 - y: 23.87765425 - z: 10.75159362 -- x: 3.13511549 - y: 23.80404808 - z: 10.10288971 -- x: -0.00135166 - y: 28.02324631 - z: 7.86111074 -- x: -0.00041269 - y: 29.21465893 - z: 3.96386436 -- x: 7.38872988 - y: -1.83843131 - z: 9.31075593 -- x: 4.25469072 - y: -1.83842924 - z: 12.2044915 -- x: 0.00139008 - y: -1.8375095 - z: 12.76352439 -- x: 0.00118076 - y: 1.36767275 - z: 11.08371687 -- x: 0.0010128 - y: 4.17300381 - z: 9.76182211 -- x: 0.00088397 - y: 7.66127894 - z: 8.9032549 -- x: 0.0007587 - y: 10.66833351 - z: 9.60998857 -- x: 0.00054434 - y: 14.13917019 - z: 10.6290711 -- x: 0.00037025 - y: 17.19870209 - z: 11.12688274 -- x: 0.00017672 - y: 20.81310695 - z: 11.20073003 -- x: 6.37805738 - y: 20.12836446 - z: 8.46599775 -- x: 6.19975695 - y: 16.80815911 - z: 8.75863336 -- x: 6.09392303 - y: 13.78451673 - z: 8.28088525 -- x: 5.81396004 - y: 10.82948586 - z: 7.59170343 -- x: 5.67589675 - y: 7.37239728 - z: 7.04781175 -- x: 5.6229397 - y: 4.30120402 - z: 7.98710121 -- x: 6.51658251 - y: 1.00320616 - z: 8.81523868 -- x: 2.90784487 - y: 10.8145338 - z: 9.36445453 -- x: 3.1353787 - y: 4.13280498 - z: 9.48359329 -- x: 3.59420968 - y: 1.33607952 - z: 10.72327448 -- x: 2.85383295 - y: 7.20978104 - z: 8.63989556 -- x: 3.69669249 - y: 17.22514737 - z: 10.57684617 -- x: 3.05173451 - y: 13.72904124 - z: 10.2126987 -- x: 3.5724037 - y: 20.29029381 - z: 10.63665947 -- x: 7.49702381 - y: 21.41133315 - z: 2.32287156 -- x: 7.91191336 - y: 18.23690022 - z: 1.65530874 -- x: 8.01628097 - y: 15.03729215 - z: 0.98243386 -- x: 7.95638556 - y: 11.78169855 - z: 0.8304708 -- x: 7.90871925 - y: 8.51014555 - z: 0.83046985 -- x: 8.00305533 - y: 4.69426217 - z: 0.83047505 -- x: 8.18310978 - y: 1.42714225 - z: 0.8304852 -- x: 8.39343911 - y: -1.83824164 - z: 0.83051365 -- x: 8.48951463 - y: -1.83844166 - z: 5.16629332 -- x: 7.59496851 - y: 11.08610671 - z: 4.65554922 -- x: 7.70174458 - y: 4.37108182 - z: 4.67018384 -- x: 8.14369098 - y: 1.28202463 - z: 4.96284079 -- x: 7.45041044 - y: 7.98683531 - z: 4.41370589 -- x: 7.94076231 - y: 17.55570873 - z: 5.00276188 -- x: 7.81258117 - y: 14.21466797 - z: 5.08267734 -- x: 7.61104993 - y: 20.74950266 - z: 5.49770801 -- x: -4.03569563 - y: 28.08244307 - z: 3.72577291 -- x: -6.62767274 - y: 24.49442143 - z: 2.9712037 -- x: -6.53635781 - y: 24.33135486 - z: 5.83911297 -- x: -4.09826233 - y: 27.00584015 - z: 7.05315498 -- x: -5.72524942 - y: 23.13913822 - z: 8.34520194 -- x: -3.13511549 - y: 23.80404808 - z: 10.10288971 -- x: -7.38872988 - y: -1.83843131 - z: 9.31075593 -- x: -4.25469072 - y: -1.83842924 - z: 12.2044915 -- x: -6.37805738 - y: 20.12836446 - z: 8.46599775 -- x: -6.19975695 - y: 16.80815911 - z: 8.75863336 -- x: -6.09392303 - y: 13.78451673 - z: 8.28088525 -- x: -5.81396004 - y: 10.82948586 - z: 7.59170343 -- x: -5.67589675 - y: 7.37239728 - z: 7.04781175 -- x: -5.6229397 - y: 4.30120402 - z: 7.98710121 -- x: -6.51658251 - y: 1.00320616 - z: 8.81523868 -- x: -2.90784487 - y: 10.8145338 - z: 9.36445453 -- x: -3.1353787 - y: 4.13280498 - z: 9.48359329 -- x: -3.59420968 - y: 1.33607952 - z: 10.72327448 -- x: -2.85383295 - y: 7.20978104 - z: 8.63989556 -- x: -3.69669249 - y: 17.22514737 - z: 10.57684617 -- x: -3.05173451 - y: 13.72904124 - z: 10.2126987 -- x: -3.5724037 - y: 20.29029381 - z: 10.63665947 -- x: -7.49702381 - y: 21.41133315 - z: 2.32287156 -- x: -7.91191336 - y: 18.23690022 - z: 1.65530874 -- x: -8.01628097 - y: 15.03729215 - z: 0.98243386 -- x: -7.95638556 - y: 11.78169855 - z: 0.8304708 -- x: -7.90871925 - y: 8.51014555 - z: 0.83046985 -- x: -8.00305533 - y: 4.69426217 - z: 0.83047505 -- x: -8.18310978 - y: 1.42714225 - z: 0.8304852 -- x: -8.39343911 - y: -1.83824164 - z: 0.83051365 -- x: -8.48951463 - y: -1.83844166 - z: 5.16629332 -- x: -7.59496851 - y: 11.08610671 - z: 4.65554922 -- x: -7.70174458 - y: 4.37108182 - z: 4.67018384 -- x: -8.14369098 - y: 1.28202463 - z: 4.96284079 -- x: -7.45041044 - y: 7.98683531 - z: 4.41370589 -- x: -7.94076231 - y: 17.55570873 - z: 5.00276188 -- x: -7.81258117 - y: 14.21466797 - z: 5.08267734 -- x: -7.61104993 - y: 20.74950266 - z: 5.49770801 diff --git a/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml deleted file mode 100644 index f4212dff..00000000 --- a/orca_core/hardware/sensing/models/touch-sensor-pinky/config.yaml +++ /dev/null @@ -1,157 +0,0 @@ -name: touch-sensor-pinky -description: ORCA Fingertip - Pinky -num_taxels: 51 -coordinates: -- x: -7.31378277 - y: -1.79988568 - z: 0.39999828 -- x: -7.35349939 - y: -1.79999907 - z: 3.44426423 -- x: -7.14890952 - y: 2.23310916 - z: 0.39995294 -- x: -7.239463 - y: 1.5965166 - z: 3.25227094 -- x: -6.96664673 - y: 5.76010809 - z: 0.39988187 -- x: -7.06807114 - y: 5.49744394 - z: 3.03648049 -- x: -6.58421581 - y: 9.77742185 - z: 0.39989891 -- x: -6.00996443 - y: -1.79999958 - z: 6.67266775 -- x: -6.16505672 - y: 1.39784473 - z: 6.53131412 -- x: -6.19950575 - y: 5.14650534 - z: 6.31004181 -- x: -6.80097742 - y: 8.9453581 - z: 2.83870363 -- x: -5.89906718 - y: 8.44673336 - z: 5.91034299 -- x: -5.71308078 - y: 13.71144591 - z: 0.40007939 -- x: -5.91467725 - y: 12.89776522 - z: 2.62718238 -- x: -3.03222976 - y: -1.79999629 - z: 8.50433192 -- x: -3.21085316 - y: 2.11997188 - z: 8.71002527 -- x: -3.71209236 - y: 6.06174158 - z: 8.34183848 -- x: -5.16324911 - y: 11.6529258 - z: 5.37155488 -- x: -3.34056505 - y: 9.91752203 - z: 7.56053006 -- x: -3.73628217 - y: 16.5905664 - z: 0.3999584 -- x: -3.45158551 - y: 15.76622462 - z: 3.67837322 -- x: -2.76185013 - y: 13.00866685 - z: 6.43839714 -- x: -1.0e-08 - y: -1.8 - z: 8.66869815 -- x: -1.0e-08 - y: 2.14667381 - z: 8.97522434 -- x: -1.0e-08 - y: 6.09991603 - z: 8.84761706 -- x: -1.0e-08 - y: 9.96543874 - z: 8.00437736 -- x: -1.0e-08 - y: 13.63052494 - z: 6.51654897 -- x: -1.0e-08 - y: 16.73346681 - z: 4.10241064 -- x: -1.0e-08 - y: 17.90374908 - z: 0.39997619 -- x: 2.76185013 - y: 13.00866685 - z: 6.43839714 -- x: 3.03222976 - y: -1.79999629 - z: 8.50433192 -- x: 3.71209236 - y: 6.06174158 - z: 8.34183848 -- x: 3.34056505 - y: 9.91752203 - z: 7.56053006 -- x: 3.45158551 - y: 15.76622462 - z: 3.67837322 -- x: 3.21085316 - y: 2.11997188 - z: 8.71002527 -- x: 3.73628217 - y: 16.5905664 - z: 0.3999584 -- x: 5.16324911 - y: 11.6529258 - z: 5.37155488 -- x: 6.00996443 - y: -1.79999958 - z: 6.67266775 -- x: 6.16505672 - y: 1.39784473 - z: 6.53131412 -- x: 6.19950575 - y: 5.14650534 - z: 6.31004181 -- x: 5.89906718 - y: 8.44673336 - z: 5.91034299 -- x: 5.91467725 - y: 12.89776522 - z: 2.62718238 -- x: 7.35349939 - y: -1.79999907 - z: 3.44426423 -- x: 7.31378277 - y: -1.79988568 - z: 0.39999828 -- x: 7.239463 - y: 1.5965166 - z: 3.25227094 -- x: 7.14890952 - y: 2.23310916 - z: 0.39995294 -- x: 7.06807114 - y: 5.49744394 - z: 3.03648049 -- x: 6.96664673 - y: 5.76010809 - z: 0.39988187 -- x: 6.58421581 - y: 9.77742185 - z: 0.39989891 -- x: 6.80097742 - y: 8.9453581 - z: 2.83870363 -- x: 5.71308078 - y: 13.71144591 - z: 0.40007939 diff --git a/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml b/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml deleted file mode 100644 index ede3ce05..00000000 --- a/orca_core/hardware/sensing/models/touch-sensor-thumb/config.yaml +++ /dev/null @@ -1,157 +0,0 @@ -name: touch-sensor-thumb -description: ORCA Fingertip - Thumb -num_taxels: 51 -coordinates: -- x: -9.98783684 - y: -0.99999966 - z: 0.83664511 -- x: -9.22014693 - y: -0.60386497 - z: 4.2141522 -- x: -9.98563473 - y: 3.01522059 - z: 0.83825563 -- x: -9.34725868 - y: 3.12493816 - z: 3.96201581 -- x: -9.86319537 - y: 7.025051 - z: 0.83668072 -- x: -9.13066251 - y: 6.90457409 - z: 4.21467613 -- x: -8.90947253 - y: 10.91402385 - z: 0.83668825 -- x: -7.08587129 - y: -0.36341653 - z: 6.95899868 -- x: -6.93554584 - y: 2.94656029 - z: 7.10574445 -- x: -7.07510324 - y: 6.26486515 - z: 6.97833064 -- x: -8.34358177 - y: 10.63405643 - z: 3.83946027 -- x: -6.78658109 - y: 9.61401604 - z: 6.5180933 -- x: -7.01397155 - y: 14.44094726 - z: 0.83664566 -- x: -6.67742466 - y: 13.86389925 - z: 3.48169799 -- x: -3.4990712 - y: -0.12231963 - z: 8.65774898 -- x: -3.66401984 - y: 3.40154211 - z: 8.6618383 -- x: -3.71268789 - y: 6.92448678 - z: 8.61804962 -- x: -5.73110824 - y: 12.7621635 - z: 5.78836754 -- x: -3.55144846 - y: 10.41168292 - z: 8.07657784 -- x: -3.93225521 - y: 16.92530102 - z: 0.83665819 -- x: -3.38716757 - y: 15.99481755 - z: 4.32509838 -- x: -2.89494885 - y: 14.00354271 - z: 6.63864407 -- x: 0.00021972 - y: 0.01158422 - z: 8.79792926 -- x: 0.00015816 - y: 3.55089765 - z: 8.83782324 -- x: 9.735e-05 - y: 7.09004225 - z: 8.82407653 -- x: 3.779e-05 - y: 10.59777145 - z: 8.38341654 -- x: 0.0 - y: 14.30397222 - z: 6.83198881 -- x: 0.0 - y: 16.63586847 - z: 4.20815607 -- x: 0.0 - y: 17.61962965 - z: 0.83666099 -- x: 2.89494885 - y: 14.00354271 - z: 6.63864407 -- x: 3.4990712 - y: -0.12231963 - z: 8.65774898 -- x: 3.71268789 - y: 6.92448678 - z: 8.61804962 -- x: 3.55144846 - y: 10.41168292 - z: 8.07657784 -- x: 3.38716757 - y: 15.99481755 - z: 4.32509838 -- x: 3.66401984 - y: 3.40154211 - z: 8.6618383 -- x: 3.93225521 - y: 16.92530102 - z: 0.83665819 -- x: 5.73110824 - y: 12.7621635 - z: 5.78836754 -- x: 7.08587129 - y: -0.36341653 - z: 6.95899868 -- x: 6.93554584 - y: 2.94656029 - z: 7.10574445 -- x: 7.07510324 - y: 6.26486515 - z: 6.97833064 -- x: 6.78658109 - y: 9.61401604 - z: 6.5180933 -- x: 6.67742466 - y: 13.86389925 - z: 3.48169799 -- x: 9.22014693 - y: -0.60386497 - z: 4.2141522 -- x: 9.98783684 - y: -0.99999966 - z: 0.83664511 -- x: 9.34725868 - y: 3.12493816 - z: 3.96201581 -- x: 9.98563473 - y: 3.01522059 - z: 0.83825563 -- x: 9.13066251 - y: 6.90457409 - z: 4.21467613 -- x: 9.86319537 - y: 7.025051 - z: 0.83668072 -- x: 8.90947253 - y: 10.91402385 - z: 0.83668825 -- x: 8.34358177 - y: 10.63405643 - z: 3.83946027 -- x: 7.01397155 - y: 14.44094726 - z: 0.83664566 diff --git a/orca_core/hardware/sensing/taxel_coordinates.py b/orca_core/hardware/sensing/taxel_coordinates.py deleted file mode 100644 index 1d9008c7..00000000 --- a/orca_core/hardware/sensing/taxel_coordinates.py +++ /dev/null @@ -1,72 +0,0 @@ -"""Config-driven taxel coordinates for tactile sensors. - -Coordinates are loaded from sensor model config files in the models/ directory. -The finger-to-model mapping is configured in models/sensor_models.yaml. -""" - -import os -from typing import TypedDict -import yaml - -from orca_core.hardware.sensing.constants import FINGER_MODELS - - -class TaxelCoord(TypedDict): - x: float - y: float - z: float - - -MODELS_DIR = os.path.join(os.path.dirname(__file__), "models") - -_model_cache: dict[str, list[TaxelCoord]] = {} - - -def _load_model_coordinates(model_name: str) -> list[TaxelCoord]: - """Load coordinates for a sensor model from its config.yaml.""" - if model_name in _model_cache: - return _model_cache[model_name] - - config_path = os.path.join(MODELS_DIR, model_name, "config.yaml") - with open(config_path, "r") as f: - config = yaml.safe_load(f) - - coords = [TaxelCoord(x=c["x"], y=c["y"], z=c["z"]) for c in config["coordinates"]] - _model_cache[model_name] = coords - return coords - - -def get_coordinates(finger: str) -> list[TaxelCoord]: - """Get taxel coordinates for a finger. - - Args: - finger: Finger name ('thumb', 'index', 'middle', 'ring', 'pinky') - - Returns: - List of coordinate dicts with 'x', 'y', 'z' keys (in mm) - """ - mapping = FINGER_MODELS - model_name = mapping.get(finger) - if model_name is None: - return [] - return _load_model_coordinates(model_name) - - -def get_all_coordinates() -> dict[str, list[TaxelCoord]]: - """Get taxel coordinates for all fingers. - - Returns: - Dict mapping finger name to list of coordinate dicts - """ - mapping = FINGER_MODELS - return {finger: _load_model_coordinates(model) for finger, model in mapping.items()} - - -def get_taxel_counts() -> dict[str, int]: - """Get the taxel count for each finger based on the current model mapping. - - Returns: - Dict mapping finger name to number of taxels - """ - mapping = FINGER_MODELS - return {finger: len(_load_model_coordinates(model)) for finger, model in mapping.items()} diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 44bc0432..ae8cb54d 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -20,6 +20,7 @@ from .calibration import CalibrationResult from .hand_config import OrcaHandConfig, OrcaHandTouchConfig from .hardware.motor_client import MotorClient +from .hardware.sensing.types import ResultantReading, TaxelReading from .utils.utils import auto_detect_port, get_and_choose_port, update_yaml if TYPE_CHECKING: @@ -1202,15 +1203,33 @@ def disconnect(self) -> None: self._sensor_client = None super().disconnect() - def get_tactile_forces(self) -> dict[str, list[float]]: - """Return latest resultant force per finger ``{finger: [fx, fy, fz]}``.""" - forces, _ = self._sensor_client.get_auto_latest() - return forces + def get_tactile_forces(self) -> ResultantReading | None: + """Return latest resultant force per finger, or ``None`` if unavailable. - def get_tactile_taxels(self) -> dict[str, list[list[float]]]: - """Return per-taxel forces ``{finger: [[fx, fy, fz], ...]}``.""" - taxels, _ = self._sensor_client.get_auto_latest_taxels() - return taxels + The returned object supports dict-style access by finger name:: + + reading["thumb"] # -> [fx, fy, fz] + + Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. + """ + forces, ts = self._sensor_client.get_auto_latest() + if forces is None: + return None + return ResultantReading(forces=forces, timestamp=ts) + + def get_tactile_taxels(self) -> TaxelReading | None: + """Return per-taxel forces, or ``None`` if unavailable. + + The returned object supports dict-style access by finger name:: + + reading["thumb"] # -> [[fx, fy, fz], ...] per taxel + + Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. + """ + taxels, ts = self._sensor_client.get_auto_latest_taxels() + if taxels is None: + return None + return TaxelReading(taxels=taxels, timestamp=ts) def start_tactile_stream( self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1 diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index 01741cd4..2a088729 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -2,7 +2,7 @@ Validates the mock's lifecycle (connect → stream → read → stop), offset logic, dynamic reconfiguration, and configuration ordering. Pure protocol codec tests -live in test_protocol.py; taxel coordinate tests live in test_taxel_coordinates.py. +live in test_protocol.py. """ import time diff --git a/tests/test_taxel_coordinates.py b/tests/test_taxel_coordinates.py deleted file mode 100644 index 9fa73d8a..00000000 --- a/tests/test_taxel_coordinates.py +++ /dev/null @@ -1,22 +0,0 @@ -"""Tests for taxel coordinate data integrity.""" - -from orca_core.hardware.sensing.constants import DEFAULT_TAXEL_COUNTS -from orca_core.hardware.sensing.taxel_coordinates import get_all_coordinates - - -class TestTaxelCoordinates: - def test_counts_match_models(self): - coords = get_all_coordinates() - for finger, expected_count in DEFAULT_TAXEL_COUNTS.items(): - assert len(coords[finger]) == expected_count, ( - f"{finger}: expected {expected_count} coordinates, got {len(coords[finger])}" - ) - - def test_coordinate_structure(self): - coords = get_all_coordinates() - for finger, taxels in coords.items(): - for i, coord in enumerate(taxels): - assert "x" in coord and "y" in coord and "z" in coord, ( - f"{finger} taxel {i}: missing x/y/z keys" - ) - assert all(isinstance(coord[k], float) for k in ("x", "y", "z")) From 3f9032c2e1f477c151180f71e861a8e62f6e46f3 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 7 Apr 2026 16:03:10 +0200 Subject: [PATCH 08/20] Simplify auto-stream error handling and reconfiguration --- orca_core/hardware/sensor_client.py | 152 +++++++++------------------- 1 file changed, 48 insertions(+), 104 deletions(-) diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index 46b8e7b7..d80928ea 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -89,7 +89,6 @@ class AutoStreamStats: parse_errors: int = 0 last_eff_len: int = 0 last_payload_len: int = 0 - consecutive_errors: int = 0 # For error-triggered reconfiguration reconfiguration_count: int = 0 # Number of times config was updated @@ -166,7 +165,6 @@ def __init__(self, # Sensor configuration (dynamic, adapts to connected sensors) self._sensor_config: SensorConfiguration | None = None - self._last_reconfigure_time: float = 0.0 # Rate limiting for reconfiguration self._auto_thread: threading.Thread | None = None self._auto_running = threading.Event() # Thread-safe flag for auto stream @@ -552,68 +550,47 @@ def _get_configuration(self) -> SensorConfiguration: logger.error(f"Failed to get sensor configuration: {e}") raise IOError(f"Failed to read sensor configuration: {e}") from e - def _reconfigure(self, force: bool = False) -> bool: - """Attempt to reconfigure sensor client with current hardware state. + def _reconfigure(self) -> bool: + """Re-query the sensor board and update the cached configuration. - This is called when errors indicate the sensor configuration may have changed - (e.g., sensor disconnected or reconnected). It rate-limits reconfiguration - to avoid thrashing on flaky connections. - - Args: - force: If True, bypass rate limiting and reconfigure immediately + Called by `_acquire_frame` when a payload-size mismatch indicates the + hardware reconfigured itself (sensor connected or disconnected). Returns: - True if reconfiguration succeeded and configuration changed, False otherwise + True if the active-sensor set changed, False if unchanged. Raises: - NoSensorsAvailableError: If no sensors are connected after reconfiguration + NoSensorsAvailableError: If no sensors are connected after reconfiguration. """ - # Rate limiting: don't reconfigure more than once per 2 seconds (unless forced) - now = time.time() - if not force and (now - self._last_reconfigure_time) < 2.0: - logger.debug("Reconfiguration rate-limited, skipping") - return False + logger.info("Attempting reconfiguration...") + new_config = self._get_configuration() - try: - logger.info("Attempting reconfiguration...") - new_config = self._get_configuration() - - # Check if configuration actually changed - if self._sensor_config is not None: - old_active = set(self._sensor_config.active_sensors) - new_active = set(new_config.active_sensors) - - if old_active == new_active: - logger.debug("Configuration unchanged, no reconfiguration needed") - return False - - # Log configuration changes - added = new_active - old_active - removed = old_active - new_active - if added: - logger.info(f"Sensors added: {', '.join(added)}") - if removed: - logger.warning(f"Sensors removed: {', '.join(removed)}") - - # Update configuration - self._sensor_config = new_config - self._last_reconfigure_time = now - - with self._auto_lock: - self._auto_stats.reconfiguration_count += 1 - self._auto_stats.consecutive_errors = 0 # Reset error counter - - # Check if we have any sensors left - if new_config.num_active_sensors == 0: - logger.error("No sensors available after reconfiguration") - raise NoSensorsAvailableError("All sensors disconnected") - - logger.info(f"Reconfiguration successful: {new_config}") - return True + if self._sensor_config is not None: + old_active = set(self._sensor_config.active_sensors) + new_active = set(new_config.active_sensors) - except Exception as e: - logger.error(f"Reconfiguration failed: {e}") - raise + if old_active == new_active: + logger.debug("Configuration unchanged, no reconfiguration needed") + return False + + added = new_active - old_active + removed = old_active - new_active + if added: + logger.info(f"Sensors added: {', '.join(added)}") + if removed: + logger.warning(f"Sensors removed: {', '.join(removed)}") + + self._sensor_config = new_config + + with self._auto_lock: + self._auto_stats.reconfiguration_count += 1 + + if new_config.num_active_sensors == 0: + logger.error("No sensors available after reconfiguration") + raise NoSensorsAvailableError("All sensors disconnected") + + logger.info(f"Reconfiguration successful: {new_config}") + return True def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: @@ -998,13 +975,8 @@ def _acquire_frame( f"Payload size mismatch: expected {expected_size}, got {len(valid)}. " "Triggering reconfiguration..." ) - try: - if self._reconfigure(force=False): - logger.info("Reconfiguration successful, continuing stream") - except NoSensorsAvailableError: - raise - except Exception as e: - logger.error(f"Reconfiguration failed: {e}") + if self._reconfigure(): + logger.info("Reconfiguration successful, continuing stream") raise FrameError("Payload size mismatch, skipping frame") # Parse payload based on mode @@ -1036,33 +1008,6 @@ def _acquire_frame( return parsed_resultant, parsed_taxels - def _handle_error_threshold_reconfiguration(self, min_sensors: int) -> None: - """Attempt reconfiguration when consecutive errors exceed threshold. - - Called by the auto-reader loop. Stops the stream if no sensors remain - or if the minimum sensor requirement is no longer met. - """ - logger.warning( - f"Consecutive errors ({self._auto_stats.consecutive_errors}) exceeded threshold. " - "Attempting reconfiguration..." - ) - try: - if self._reconfigure(force=False): - logger.info("Reconfiguration successful") - if self._sensor_config.num_active_sensors < min_sensors: - logger.error( - f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " - f"need {min_sensors}. Stopping stream." - ) - self._auto_running.clear() - except NoSensorsAvailableError: - logger.error("No sensors available, stopping stream") - self._auto_running.clear() - except Exception as e: - logger.error(f"Reconfiguration failed: {e}") - with self._auto_lock: - self._auto_stats.consecutive_errors = 0 - def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_sensors: int): """Background thread that continuously reads and parses auto-stream frames. @@ -1070,13 +1015,20 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso stats. Subclasses override _acquire_frame() to change the data source while inheriting the shared loop logic. + Error handling: + - FrameError: recoverable (bad LRC / parse error / size mismatch). Count + and continue. Size-mismatch frames trigger reconfiguration inside + _acquire_frame. + - NoSensorsAvailableError: terminal. Stop the stream cleanly. + - IOError: serial-level hiccup. Count, back off briefly, continue. + - Any other Exception: unexpected (likely a programming bug). Log the + traceback and stop the stream loudly rather than rotting silently. + Args: parse_resultant: Whether to parse resultant force data parse_taxels: Whether to parse individual taxel data min_sensors: Minimum number of sensors required to continue streaming """ - ERROR_THRESHOLD = 5 - while self._auto_running.is_set(): try: parsed_resultant, parsed_taxels = self._acquire_frame( @@ -1093,7 +1045,6 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso self._auto_latest_ts = time.time() self._auto_stats.frames_ok += 1 self._auto_stats.parse_ok += 1 - self._auto_stats.consecutive_errors = 0 except FrameError as e: with self._auto_lock: @@ -1102,7 +1053,6 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso else: self._auto_stats.frames_ok += 1 self._auto_stats.parse_errors += 1 - self._auto_stats.consecutive_errors += 1 except NoSensorsAvailableError: logger.error("No sensors available, stopping stream") @@ -1116,18 +1066,12 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso logger.warning(f"IO error in auto reader: {e}") with self._auto_lock: self._auto_stats.resyncs += 1 - self._auto_stats.consecutive_errors += 1 - time.sleep(0.01) - - except Exception as e: - logger.error(f"Unexpected error in auto reader: {e}", exc_info=True) - with self._auto_lock: - self._auto_stats.resyncs += 1 - self._auto_stats.consecutive_errors += 1 time.sleep(0.01) - if self._auto_stats.consecutive_errors >= ERROR_THRESHOLD: - self._handle_error_threshold_reconfiguration(min_sensors) + except Exception: + logger.exception("Unexpected error in auto reader, stopping stream") + self._auto_running.clear() + break logger.info("Auto reader loop exited") From a0c5d355d3dc772d2fe7fd4ca60f2b2f034010a2 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 7 Apr 2026 16:50:06 +0200 Subject: [PATCH 09/20] Address review feedback: rename stats fields, narrow excepts, remove dead code --- orca_core/hardware/mock_sensor_client.py | 7 +- orca_core/hardware/sensor_client.py | 155 ++++++++--------------- 2 files changed, 55 insertions(+), 107 deletions(-) diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 2ef53af3..8d54ef04 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -193,12 +193,7 @@ def read_auto_data_type(self) -> dict: def _write_register(self, address: int, data: bytes, response_timeout_s: float = 0.5) -> None: pass - def reboot(self) -> None: - if not self.is_connected: - raise OSError("Must call connect() first.") - logger.info("[MOCK] Sensor reboot simulated") - - # ========================================================================= +# ========================================================================= # Force Reading # ========================================================================= diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index d80928ea..b1eb83a7 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -22,7 +22,6 @@ PROTOCOL_HEADER_AUTO, RESPONSE_META_SIZE, AUTO_FRAME_META_SIZE, - ADDR_RESET, ADDR_CONNECTED_SENSORS_START, ADDR_CONNECTED_SENSORS_LENGTH, ADDR_NUM_TAXELS_START, @@ -81,15 +80,23 @@ def __init__(self, message: str, bad_lrc: bool = False): @dataclass class AutoStreamStats: + """Diagnostic counters for the auto-stream reader loop. + + Attributes: + frames_ok: Frames received and decoded successfully. + frames_bad_checksum: Frames rejected due to checksum (LRC) mismatch. + parse_errors: Frames received intact but whose payload failed to decode. + resyncs: Times the reader had to resync after IO errors or bad framing. + reconfiguration_count: Times the sensor configuration was re-queried + after a payload-size mismatch. + last_error_code: Most recent sensor-reported error code (0 = no error). + """ frames_ok: int = 0 - frames_bad_lrc: int = 0 + frames_bad_checksum: int = 0 + parse_errors: int = 0 resyncs: int = 0 + reconfiguration_count: int = 0 last_error_code: int = 0 - parse_ok: int = 0 - parse_errors: int = 0 - last_eff_len: int = 0 - last_payload_len: int = 0 - reconfiguration_count: int = 0 # Number of times config was updated @dataclass @@ -215,11 +222,11 @@ def connect(self): try: self._sensor_config = self._get_configuration() logger.info(f"Initial configuration: {self._sensor_config}") - except Exception as e: + except IOError as e: logger.warning(f"Failed to get initial configuration: {e}") # Don't fail connection, config will be retrieved when starting auto-stream - except Exception as e: + except (serial.SerialException, OSError) as e: raise ConnectionError(f"Failed to connect to sensor at {self.port}: {e}") from e def disconnect(self): @@ -434,17 +441,19 @@ def read_num_taxels(self) -> dict[str, int]: return decode_num_taxels(data, self._sensor_id_to_finger) def read_auto_data_type(self) -> dict: - """Read the auto data type register. + """Read the auto-stream data-type register. + + Returns the configured payload format for auto-stream frames (which of + resultant force / individual taxels are included). Returns: - Dictionary with raw binary value and parsed flags + Dict with the raw register byte and decoded resultant/taxels flags. Raises: - OSError: If not connected to sensor + OSError: If not connected to sensor. """ 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) @@ -462,7 +471,7 @@ def _read_raw_resultant(self) -> dict[str, list[float]]: if self._sensor_config is None: try: self._sensor_config = self._get_configuration() - except Exception as e: + except IOError as e: logger.error(f"Failed to get configuration: {e}") # Fall back to static parsing using default module indices data = self._read_register(ADDR_RESULTANT_FORCE_START, RESULTANT_BLOCK_SIZE) @@ -546,7 +555,7 @@ def _get_configuration(self) -> SensorConfiguration: logger.info(f"Configuration captured: {config}") return config - except Exception as e: + 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 @@ -632,101 +641,51 @@ def disable_auto_data_transmission(self) -> None: self._write_register(ADDR_AUTO_ENABLE, REGISTER_DISABLE) - def reboot(self) -> None: - """Reboot the sensor. - - Raises: - OSError: If not connected to sensor - """ - if not self.is_connected: - raise OSError("Must call connect() first.") - - self._write_register(ADDR_RESET, REGISTER_ENABLE) - def get_auto_latest(self): - """Get the most recently parsed auto-stream resultant force data (thread-safe). + """Thread-safe snapshot of the most recent resultant-force frame. - Returns a snapshot of the latest force data received from the auto-stream. - This is updated by the background thread at ~1kHz when auto streaming is active. + Updated by the background reader thread while auto-stream is active. Returns: - Tuple of (parsed_data, timestamp): - - parsed_data: Dictionary mapping finger names to [fx, fy, fz] forces (Newtons) - None if no data received yet or resultant mode not enabled - - timestamp: Unix timestamp (time.time()) when data was received - None if no data received yet - - Example: - >>> client.start_auto_stream(resultant=True) - >>> time.sleep(0.1) # Let some data arrive - >>> forces, ts = client.get_auto_latest() - >>> print(forces) - {'index': [0.1, -0.2, 1.5]} + (forces, timestamp): `forces` is a {finger: [fx, fy, fz]} dict in + Newtons, or None if no frame has arrived or resultant mode is off. + `timestamp` is the wall-clock time of receipt, or None. """ with self._auto_lock: return self._auto_latest, self._auto_latest_ts def get_auto_latest_taxels(self): - """Get the most recently parsed auto-stream taxel data (thread-safe). + """Thread-safe snapshot of the most recent per-taxel frame. - Returns a snapshot of the latest taxel data received from the auto-stream. - Only available when auto-stream was started with taxels=True. + Only populated when auto-stream was started with `taxels=True`. Returns: - Tuple of (parsed_data, timestamp): - - parsed_data: Dictionary mapping finger names to list of taxel force vectors - Each taxel is [fx, fy, fz] in Newtons - None if no data received yet or taxel mode not enabled - - timestamp: Unix timestamp (time.time()) when data was received - None if no data received yet - - Example: - >>> client.start_auto_stream(resultant=False, taxels=True) - >>> time.sleep(0.1) - >>> taxels, ts = client.get_auto_latest_taxels() - >>> print(taxels) - {'index': [[0.1, -0.2, 0.5], [0.0, 0.1, 0.3], ...]} # list of [fx, fy, fz] per taxel + (taxels, timestamp): `taxels` is a {finger: [[fx, fy, fz], ...]} + dict (one [fx, fy, fz] per taxel, in Newtons), or None if no frame + has arrived or taxel mode is off. `timestamp` is the wall-clock + time of receipt, or None. """ with self._auto_lock: return self._auto_latest_taxels, self._auto_latest_ts def get_auto_latest_all(self): - """Get both resultant forces and taxels from latest auto-stream data (thread-safe). + """Thread-safe snapshot of both resultant and taxel data in one call. - Returns all available data from the auto-stream. Useful when running in - combined mode (resultant=True, taxels=True). + Useful in combined mode (`resultant=True, taxels=True`) to get a + consistent view without two separate locked reads. Returns: - Tuple of (resultant_forces, taxels, timestamp): - - resultant_forces: Dict mapping finger names to [fx, fy, fz], or None - - taxels: Dict mapping finger names to taxel value lists, or None - - timestamp: Unix timestamp when data was received, or None - - Example: - >>> client.start_auto_stream(resultant=True, taxels=True) - >>> time.sleep(0.1) - >>> forces, taxels, ts = client.get_auto_latest_all() + (forces, taxels, timestamp). Any field may be None depending on + the active stream mode and whether a frame has arrived yet. """ with self._auto_lock: return self._auto_latest, self._auto_latest_taxels, self._auto_latest_ts def get_auto_stats(self): - """Get auto-stream statistics (thread-safe). + """Thread-safe snapshot of auto-stream diagnostics. - Returns diagnostic information about the auto-stream performance: - - frames_ok: Number of successfully received frames - - frames_bad_lrc: Number of frames with checksum errors - - resyncs: Number of times the reader had to resync after errors - - parse_ok: Number of successfully parsed frames - - parse_errors: Number of frames that couldn't be parsed - - last_error_code: Most recent error code from sensor (0 = no error) - - Returns: - AutoStreamStats dataclass with statistics - - Example: - >>> stats = client.get_auto_stats() - >>> print(f"Success rate: {stats.frames_ok}/{stats.frames_ok + stats.frames_bad_lrc}") + See `AutoStreamStats` for the meaning of each field. Useful for health + monitoring (frame rate, checksum errors, reconfigurations). """ with self._auto_lock: return self._auto_stats @@ -750,10 +709,6 @@ def clear_taxel_offsets(self) -> None: self._taxel_offsets = None self._resultant_offsets = None - def get_taxel_offsets(self) -> dict | None: - """Return current per-taxel offsets (for saving to YAML).""" - return self._taxel_offsets - def capture_taxel_offsets(self, num_samples: int = 100) -> dict: """Capture live baseline offsets by averaging current sensor readings. @@ -778,6 +733,7 @@ def capture_taxel_offsets(self, num_samples: int = 100) -> dict: # Wait for at least one raw frame to flush old offset-applied data time.sleep(0.01) + succeeded = False try: # Collect unique frames by checking timestamps frames = [] @@ -807,12 +763,13 @@ def capture_taxel_offsets(self, num_samples: int = 100) -> dict: offsets[finger] = avg self.set_taxel_offsets(offsets) + succeeded = True return offsets - except Exception: - # Restore previous offsets on failure - self._taxel_offsets = prev_taxel - self._resultant_offsets = prev_resultant - raise + finally: + if not succeeded: + # Restore previous offsets on failure + 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.""" @@ -964,8 +921,6 @@ def _acquire_frame( # Update serial-specific stats with self._auto_lock: self._auto_stats.last_error_code = err_code - self._auto_stats.last_eff_len = eff_len - self._auto_stats.last_payload_len = len(valid) # Check for payload size mismatch (indicates config change) if self._sensor_config: @@ -1044,14 +999,12 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso self._auto_latest_taxels = parsed_taxels self._auto_latest_ts = time.time() self._auto_stats.frames_ok += 1 - self._auto_stats.parse_ok += 1 except FrameError as e: with self._auto_lock: if e.bad_lrc: - self._auto_stats.frames_bad_lrc += 1 + self._auto_stats.frames_bad_checksum += 1 else: - self._auto_stats.frames_ok += 1 self._auto_stats.parse_errors += 1 except NoSensorsAvailableError: @@ -1124,7 +1077,7 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se # Get initial sensor configuration try: self._sensor_config = self._get_configuration() - except Exception as e: + except IOError as e: raise OSError(f"Failed to get sensor configuration: {e}") from e # Check minimum sensor requirement @@ -1149,7 +1102,7 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se # Try to disable auto mode first (in case it was left enabled) try: self.disable_auto_data_transmission() - except Exception: + except IOError: pass # If this fails, robust _write_register will handle AA56 frames # Configure which data types to include in auto frames @@ -1196,7 +1149,7 @@ def stop_auto_stream(self): if self.is_connected: try: self.disable_auto_data_transmission() - except Exception: + except IOError: pass # Ignore errors (e.g., if sensor disconnected) # Reset cached data From b8119e4703aa8ff5a8f5a27faa4624d0cd611308 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Mon, 27 Apr 2026 15:50:15 +0200 Subject: [PATCH 10/20] Add wait_for_first_frame and taxel_counts to MockSensorClient Lets tests synchronize on auto-stream startup without polling, and configure per-finger taxel counts via the constructor instead of mutating private state. --- orca_core/hardware/mock_sensor_client.py | 30 ++++++++++++++++++++++-- orca_core/hardware/sensor_client.py | 8 +++++-- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 8d54ef04..f0a91daf 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -10,6 +10,7 @@ from __future__ import annotations from collections.abc import Callable +import threading import time import logging @@ -55,6 +56,7 @@ def __init__( 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, @@ -65,11 +67,15 @@ def __init__( if connected_sensors is None: connected_sensors = list(FINGER_NAMES) + self._taxel_counts_per_finger: dict[str, int] = ( + 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: DEFAULT_TAXEL_COUNTS[f] if f in connected_sensors else 0 + f: self._taxel_counts_per_finger[f] if f in connected_sensors else 0 for f in FINGER_NAMES } @@ -87,6 +93,10 @@ def __init__( # to throttle to ~1kHz for demos or UI prototyping. self._auto_rate_hz = auto_rate_hz + # Set by _acquire_frame after the first frame is produced. Lets tests + # synchronize on stream start without polling/sleeping. + self._first_frame_event = threading.Event() + # ========================================================================= # Mock Control Methods # ========================================================================= @@ -204,6 +214,22 @@ def _read_raw_resultant(self) -> ResultantForces: # 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. + + Lets tests synchronize on stream startup without polling or sleeps. + Raises TimeoutError if no frame arrives in `timeout` seconds. + """ + 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, @@ -231,7 +257,7 @@ def _update_connectivity(self, sensors: list[str]) -> None: """Update simulated connectivity and reconfigure if connected.""" self._sim_connected = {f: f in sensors for f in FINGER_NAMES} self._sim_taxel_counts = { - f: DEFAULT_TAXEL_COUNTS[f] if self._sim_connected[f] else 0 + f: self._taxel_counts_per_finger[f] if self._sim_connected[f] else 0 for f in FINGER_NAMES } if self._connected: diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index b1eb83a7..95529cc5 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -460,7 +460,7 @@ def read_auto_data_type(self) -> dict: def _read_raw_resultant(self) -> dict[str, list[float]]: """Read raw resultant forces from hardware (no offset application). - Subclasses (e.g. MockSensorClient) override this to return simulated + MockSensorClient overrides this to return simulated data. The public read_resultant_force() method calls this, then applies zeroing offsets. @@ -487,7 +487,6 @@ def read_resultant_force(self) -> dict[str, list[float]]: """Read resultant force from all connected fingertip sensors. Calls _read_raw_resultant() for data, then applies zeroing offsets. - Subclasses should override _read_raw_resultant(), not this method. Returns: Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons @@ -795,6 +794,10 @@ def _apply_resultant_offsets(self, forces: dict) -> None: 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 called after a frame is stored in _auto_latest. Subclasses + may override to signal frame availability (e.g. for tests).""" + def _apply_stream_offsets( self, parsed_resultant: dict | None, @@ -999,6 +1002,7 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso 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: From 5305852145e930b572ec01e9130a91be982fd373 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Mon, 27 Apr 2026 15:51:07 +0200 Subject: [PATCH 11/20] Refactor tactile sensor tests to flat parametrized style Replace class-based tests with module-level functions, parametrize where it tightens coverage, and use wait_for_first_frame() instead of the polling helpers. --- tests/test_protocol.py | 741 +++++++++++++++++------------------ tests/test_tactile_sensor.py | 410 ++++++++----------- 2 files changed, 534 insertions(+), 617 deletions(-) diff --git a/tests/test_protocol.py b/tests/test_protocol.py index b9e7bb80..ef13eee7 100644 --- a/tests/test_protocol.py +++ b/tests/test_protocol.py @@ -18,7 +18,6 @@ compute_resultant_payload_size, compute_taxel_payload_size, compute_combined_payload_size, - compute_distal_module_index, decode_resultant_auto, decode_taxels_auto, decode_combined_auto, @@ -39,489 +38,471 @@ BYTES_PER_RESULTANT, BYTES_PER_TAXEL, MAX_AUTO_FRAME_EFF_LEN, - MIN_READ_RESPONSE_SIZE, - MIN_WRITE_RESPONSE_SIZE, - MODULES_PER_SLOT, - DISTAL_MODULE_OFFSET, ) +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 # --------------------------------------------------------------------------- -class TestCalculateChecksum: - def test_known_value(self): - assert calculate_checksum(b"\x01\x02\x03") == 0xFA +def test_checksum_known_value(): + assert calculate_checksum(b"\x01\x02\x03") == 0xFA + + +def test_checksum_round_trip(): + frame = b"\xAA\x55\x00\x03\x10\x00\x04\x00" + checksum = calculate_checksum(frame) + assert (sum(frame) + checksum) & 0xFF == 0 + - def test_round_trip(self): - frame = b"\xAA\x55\x00\x03\x10\x00\x04\x00" - checksum = calculate_checksum(frame) - assert (sum(frame) + checksum) & 0xFF == 0 +def test_checksum_empty_frame(): + assert calculate_checksum(b"") == 0 - def test_empty_frame(self): - assert calculate_checksum(b"") == 0 - def test_single_byte(self): - assert calculate_checksum(b"\x01") == 0xFF +def test_checksum_single_byte(): + assert calculate_checksum(b"\x01") == 0xFF -class TestValidateAutoFrameLrc: - def test_valid_frame(self): - meta = b"\x00" + (3).to_bytes(2, "little") - payload = b"\x00\x01\x02" - frame_wo_lrc = b"\xAA\x56" + meta + payload - lrc = calculate_checksum(frame_wo_lrc) - assert validate_auto_frame_lrc(meta, payload, lrc) is True +def test_validate_auto_frame_lrc_valid(): + meta = b"\x00" + (3).to_bytes(2, "little") + payload = b"\x00\x01\x02" + frame_wo_lrc = b"\xAA\x56" + meta + payload + lrc = calculate_checksum(frame_wo_lrc) + assert validate_auto_frame_lrc(meta, payload, lrc) is True - def test_invalid_lrc(self): - meta = b"\x00" + (3).to_bytes(2, "little") - payload = b"\x00\x01\x02" - assert validate_auto_frame_lrc(meta, payload, 0xFF) is False + +def test_validate_auto_frame_lrc_invalid(): + meta = b"\x00" + (3).to_bytes(2, "little") + payload = b"\x00\x01\x02" + assert validate_auto_frame_lrc(meta, payload, 0xFF) is False # --------------------------------------------------------------------------- -# Frame Size Helpers +# Frame size helpers # --------------------------------------------------------------------------- -class TestReadResponseBodySize: - def test_known_value(self): - # count=4 → meta(6) + data(4) + LRC(1) = 11 - assert read_response_body_size(4) == 11 +def test_read_response_body_size_known_value(): + # count=4 → meta(6) + data(4) + LRC(1) = 11 + assert read_response_body_size(4) == 11 + - def test_single_byte(self): - assert read_response_body_size(1) == 8 +def test_read_response_body_size_single_byte(): + assert read_response_body_size(1) == 8 # --------------------------------------------------------------------------- -# Frame Builders +# Frame builders # --------------------------------------------------------------------------- -class TestBuildReadRequest: - def test_structure(self): - frame = build_read_request(address=0x0010, count=4) - assert frame[:2] == PROTOCOL_HEADER_REQUEST - assert frame[2] == 0x00 # reserved - assert frame[3] == FUNC_CODE_READ - assert int.from_bytes(frame[4:6], "little") == 0x0010 - assert int.from_bytes(frame[6:8], "little") == 4 +def test_build_read_request_structure(): + 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 - def test_lrc_valid(self): - frame = build_read_request(address=0x0010, count=4) - assert calculate_checksum(frame[:-1]) == frame[-1] +def test_build_read_request_lrc_valid(): + frame = build_read_request(address=0x0010, count=4) + assert calculate_checksum(frame[:-1]) == frame[-1] -class TestBuildWriteRequest: - def test_structure(self): - frame = build_write_request(address=0x0017, data=b"\x01") - assert frame[:2] == PROTOCOL_HEADER_REQUEST - assert frame[2] == 0x00 # reserved - 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 # data byte - def test_lrc_valid(self): - frame = build_write_request(address=0x0017, data=b"\x01") - assert calculate_checksum(frame[:-1]) == frame[-1] +def test_build_write_request_structure(): + 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 -# --------------------------------------------------------------------------- -# Frame Parsers — response frames -# --------------------------------------------------------------------------- +def test_build_write_request_lrc_valid(): + frame = build_write_request(address=0x0017, data=b"\x01") + assert calculate_checksum(frame[:-1]) == frame[-1] -def _build_read_response(data: bytes) -> bytes: - """Helper: build a valid read response frame for testing.""" - 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 test_build_read_request_zero_count_raises(): + with pytest.raises(ValueError, match="count must be > 0"): + build_read_request(address=0x0010, count=0) + + +def test_build_read_request_negative_count_raises(): + with pytest.raises(ValueError, match="count must be > 0"): + build_read_request(address=0x0010, count=-1) + + +def test_build_read_request_address_overflow_raises(): + with pytest.raises(ValueError, match="address"): + build_read_request(address=0x10000, count=1) -def _build_write_response(status: int) -> bytes: - """Helper: build a valid write response frame for testing.""" - 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)]) +def test_build_read_request_negative_address_raises(): + with pytest.raises(ValueError, match="address"): + build_read_request(address=-1, count=1) -class TestParseReadResponse: - def test_extracts_data(self): - frame = _build_read_response(b"\xAB\xCD\xEF\x01") - assert parse_read_response(frame) == b"\xAB\xCD\xEF\x01" - - def test_single_byte(self): - frame = _build_read_response(b"\x42") - assert parse_read_response(frame) == b"\x42" - - def test_bad_lrc_raises(self): - frame = bytearray(_build_read_response(b"\x01\x02")) - frame[-1] ^= 0xFF # corrupt LRC - with pytest.raises(IOError, match="LRC mismatch"): - parse_read_response(bytes(frame)) - - def test_too_short_raises(self): - with pytest.raises(IOError, match="too short"): - parse_read_response(b"\xAA\x55\x00") - - def test_wrong_func_code_raises(self): - 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_wrong_header_raises(self): - frame = bytearray(_build_read_response(b"\x01\x02")) - frame[0:2] = PROTOCOL_HEADER_AUTO # AA 56 instead of AA 55 - frame[-1] = calculate_checksum(bytes(frame[:-1])) # fix LRC - with pytest.raises(IOError, match="Expected response header"): - parse_read_response(bytes(frame)) - - -class TestParseWriteResponse: - def test_success(self): - frame = _build_write_response(status=0x00) - parse_write_response(frame) # should not raise - - def test_failure_status_raises(self): - frame = _build_write_response(status=0x01) - with pytest.raises(IOError, match="Write failed"): - parse_write_response(frame) - - def test_bad_lrc_raises(self): - frame = bytearray(_build_write_response(status=0x00)) - frame[-1] ^= 0xFF - with pytest.raises(IOError, match="LRC mismatch"): - parse_write_response(bytes(frame)) - - def test_too_short_raises(self): - with pytest.raises(IOError, match="too short"): - parse_write_response(b"\xAA\x55\x00") - - def test_truncated_payload_raises(self): - # Build a frame that 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_wrong_header_raises(self): - 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)) - - -class TestExtractWriteResponseDataLength: - def test_known_value(self): - # 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_wrong_size_raises(self): - with pytest.raises(ValueError, match="must be 6 bytes"): - extract_write_response_data_length(b"\x00\x00\x00\x00") + +def test_build_write_request_empty_data_raises(): + with pytest.raises(ValueError, match="data must not be empty"): + build_write_request(address=0x0017, data=b"") + + +def test_build_write_request_address_overflow_raises(): + with pytest.raises(ValueError, match="address"): + build_write_request(address=0x10000, data=b"\x01") # --------------------------------------------------------------------------- -# Frame Parsers — auto-stream frames +# Response frame parsers # --------------------------------------------------------------------------- -class TestExtractAutoFrameEffLen: - def test_known_value(self): - # reserved(1) + eff_len(2 LE) = 3 bytes - meta = b"\x00" + (42).to_bytes(2, "little") - assert extract_auto_frame_eff_len(meta) == 42 +def test_parse_read_response_extracts_data(): + frame = _build_read_response(b"\xAB\xCD\xEF\x01") + assert parse_read_response(frame) == b"\xAB\xCD\xEF\x01" - def test_max_valid(self): - meta = b"\x00" + MAX_AUTO_FRAME_EFF_LEN.to_bytes(2, "little") - assert extract_auto_frame_eff_len(meta) == MAX_AUTO_FRAME_EFF_LEN - def test_exceeds_max_raises(self): - 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) +def test_parse_read_response_single_byte(): + frame = _build_read_response(b"\x42") + assert parse_read_response(frame) == b"\x42" -class TestSplitAutoPayload: - def test_splits_error_code_and_data(self): - err, data = unpack_auto_payload(b"\x00\x01\x02\x03") - assert err == 0 - assert data == b"\x01\x02\x03" +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_nonzero_error_code(self): - err, data = unpack_auto_payload(b"\x05\xAB") - assert err == 5 - assert data == b"\xAB" - def test_error_code_only(self): - err, data = unpack_auto_payload(b"\x01") - assert err == 1 - assert data == b"" +def test_parse_read_response_too_short_raises(): + with pytest.raises(IOError, match="too short"): + parse_read_response(b"\xAA\x55\x00") -# --------------------------------------------------------------------------- -# Payload Size Computation -# --------------------------------------------------------------------------- +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)) + -class TestPayloadSizeComputation: - def test_resultant_size(self): - assert compute_resultant_payload_size(3) == 3 * BYTES_PER_RESULTANT +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_resultant_size_zero(self): - assert compute_resultant_payload_size(0) == 0 - def test_taxel_size(self): - active = ["thumb", "index"] - num_taxels = {"thumb": 51, "index": 87} - assert compute_taxel_payload_size(active, num_taxels) == (51 + 87) * BYTES_PER_TAXEL +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_taxel_size_missing_finger_raises(self): - with pytest.raises(KeyError): - compute_taxel_payload_size(["thumb"], {"index": 87}) - def test_combined_size(self): - 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 +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") # --------------------------------------------------------------------------- -# Module Index Computation +# Auto-stream frame parsers # --------------------------------------------------------------------------- -class TestComputeDistalModuleIndex: - def test_slot_zero(self): - assert compute_distal_module_index(0) == DISTAL_MODULE_OFFSET +def test_extract_auto_frame_eff_len_known_value(): + # reserved(1) + eff_len(2 LE) = 3 bytes + meta = b"\x00" + (42).to_bytes(2, "little") + assert extract_auto_frame_eff_len(meta) == 42 + + +def test_extract_auto_frame_eff_len_max_valid(): + meta = b"\x00" + MAX_AUTO_FRAME_EFF_LEN.to_bytes(2, "little") + assert extract_auto_frame_eff_len(meta) == MAX_AUTO_FRAME_EFF_LEN + - def test_slot_four(self): - assert compute_distal_module_index(4) == 4 * MODULES_PER_SLOT + DISTAL_MODULE_OFFSET +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) + + +def test_unpack_auto_payload_splits_error_and_data(): + err, data = unpack_auto_payload(b"\x00\x01\x02\x03") + assert err == 0 + assert data == b"\x01\x02\x03" + + +def test_unpack_auto_payload_nonzero_error_code(): + err, data = unpack_auto_payload(b"\x05\xAB") + assert err == 5 + assert data == b"\xAB" + + +def test_unpack_auto_payload_error_code_only(): + err, data = unpack_auto_payload(b"\x01") + assert err == 1 + assert data == b"" # --------------------------------------------------------------------------- -# Payload Decoders — auto-stream +# Payload size computation # --------------------------------------------------------------------------- -class TestDecodeResultantAuto: - def test_known_values(self): - data = struct.pack(", little-endian, ÷10 → N +# Taxel per element: , ÷10 → N +# Combined frames interleave: [resultant_sensor_i, taxels_sensor_i, ...] # --------------------------------------------------------------------------- -class TestDecodeResultantBlock: - def test_known_values(self): - # Module index for thumb (slot 0): 0*4+2 = 2, byte offset = 12 - data = b"\x00" * 168 - data_mut = bytearray(data) - struct.pack_into(" SensorConfiguration: - """Build a SensorConfiguration for testing.""" if taxel_counts is None: - taxel_counts = EXPECTED_TAXEL_COUNTS + 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} @@ -91,116 +77,90 @@ def _make_config( # --------------------------------------------------------------------------- -# Mock client — resultant forces +# Resultant forces — round-trip per finger # --------------------------------------------------------------------------- -class TestMockResultantForces: - def test_values_round_trip(self): - """Values set via set_mock_forces come back through the stream.""" - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - mock.set_mock_forces({ - "thumb": [1.5, -2.0, 3.0], - "index": [0.0, 0.0, 0.0], - "middle": [-1.0, 0.5, 10.0], - }) - mock.start_auto_stream(resultant=True, taxels=False) - result, ts = _poll_auto_latest(mock) - mock.stop_auto_stream() - mock.disconnect() - - assert ts is not None - assert result["thumb"] == [1.5, -2.0, 3.0] - assert result["index"] == [0.0, 0.0, 0.0] - assert result["middle"] == [-1.0, 0.5, 10.0] - # Fingers without explicit mock data get default [1.0, 1.0, 1.0] - assert result["ring"] == [1.0, 1.0, 1.0] - - def test_subset_only_returns_connected(self): - """Only connected sensors appear in output.""" - subset = ["thumb", "pinky"] - mock = MockSensorClient(connected_sensors=subset) - mock.connect() - mock.set_mock_forces({"thumb": [1.0, 0.0, 0.0], "pinky": [0.0, 1.0, 0.0]}) - mock.start_auto_stream(resultant=True, taxels=False) - result, _ = _poll_auto_latest(mock) - mock.stop_auto_stream() - mock.disconnect() - - assert set(result.keys()) == set(subset) - assert result["thumb"] == [1.0, 0.0, 0.0] - assert result["pinky"] == [0.0, 1.0, 0.0] - - -# --------------------------------------------------------------------------- -# Mock client — taxel data -# --------------------------------------------------------------------------- - -class TestMockTaxelData: - def test_taxel_counts_match_sensor_models(self): - """Each finger returns the expected number of taxels.""" - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - mock.start_auto_stream(resultant=False, taxels=True) - result, _ = _poll_auto_latest_taxels(mock) - mock.stop_auto_stream() - mock.disconnect() +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(ALL_FINGERS) - for finger in ALL_FINGERS: - assert len(result[finger]) == EXPECTED_TAXEL_COUNTS[finger], ( - f"{finger}: expected {EXPECTED_TAXEL_COUNTS[finger]} taxels, " - f"got {len(result[finger])}" - ) + assert set(result.keys()) == set(subset) # --------------------------------------------------------------------------- -# Mock client — combined mode +# Combined mode # --------------------------------------------------------------------------- -class TestMockCombinedMode: - def test_both_resultant_and_taxels_returned(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - mock.set_mock_forces({f: [1.0, 0.0, 0.5] for f in ALL_FINGERS}) - mock.start_auto_stream(resultant=True, taxels=True) - forces, taxels, ts = _poll_auto_latest_all(mock) - mock.stop_auto_stream() - mock.disconnect() +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 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]) == EXPECTED_TAXEL_COUNTS[finger] + 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] # --------------------------------------------------------------------------- -# Mock client — provider injection +# Provider injection # --------------------------------------------------------------------------- -class TestProviderInjection: - def test_custom_resultant_provider(self): - call_count = 0 - def counting_provider(): - nonlocal call_count - call_count += 1 - return {"thumb": [float(call_count), 0.0, 0.0]} - +@pytest.mark.parametrize( + "kind", + ["resultant", "taxel"], +) +def test_custom_provider_is_used(kind): + if kind == "resultant": mock = MockSensorClient( connected_sensors=["thumb"], - resultant_provider=counting_provider, + resultant_provider=lambda: {"thumb": [42.0, 0.0, 0.0]}, ) mock.connect() mock.start_auto_stream(resultant=True, taxels=False) - result, _ = _poll_auto_latest(mock) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest() mock.stop_auto_stream() mock.disconnect() - - assert call_count > 0 - assert result["thumb"][0] > 0 # Provider was called at least once - - def test_custom_taxel_provider(self): + assert result["thumb"] == [42.0, 0.0, 0.0] + else: marker = [[99.0, 88.0, 77.0]] mock = MockSensorClient( connected_sensors=["thumb"], @@ -208,156 +168,132 @@ def test_custom_taxel_provider(self): ) mock.connect() mock.start_auto_stream(resultant=False, taxels=True) - result, _ = _poll_auto_latest_taxels(mock) + mock.wait_for_first_frame() + result, _ = mock.get_auto_latest_taxels() mock.stop_auto_stream() mock.disconnect() - assert result["thumb"] == marker # --------------------------------------------------------------------------- -# Mock client — dynamic reconfiguration +# Dynamic reconfiguration # --------------------------------------------------------------------------- -class TestDynamicReconfiguration: - def test_simulate_dropout_removes_sensor(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - assert mock._sensor_config.num_active_sensors == 5 +def test_simulate_dropout_removes_sensors(mock): + assert mock._sensor_config.num_active_sensors == 5 + mock.simulate_dropout(["index", "ring"]) + assert mock._sensor_config.num_active_sensors == 3 + assert "index" not in mock._sensor_config.active_sensors + assert "ring" not in mock._sensor_config.active_sensors - mock.simulate_dropout(["index", "ring"]) - assert mock._sensor_config.num_active_sensors == 3 - assert "index" not in mock._sensor_config.active_sensors - assert "ring" not in mock._sensor_config.active_sensors - def test_set_connected_sensors_updates_config(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - - mock.set_connected_sensors(["thumb"]) - assert mock._sensor_config.active_sensors == ["thumb"] - assert mock._sensor_config.num_active_sensors == 1 +def test_set_connected_sensors_updates_config(mock): + mock.set_connected_sensors(["thumb"]) + assert mock._sensor_config.active_sensors == ["thumb"] + assert mock._sensor_config.num_active_sensors == 1 - def test_dropout_clears_mock_data(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - mock.set_mock_forces({"index": [5.0, 0.0, 0.0]}) - mock.simulate_dropout(["index"]) - # index mock data should be cleared - assert "index" not in mock._mock_forces +def test_dropout_clears_mock_data(mock): + mock.set_mock_forces({"index": [5.0, 0.0, 0.0]}) + mock.simulate_dropout(["index"]) + assert "index" not in mock._mock_forces # --------------------------------------------------------------------------- # Offset logic # --------------------------------------------------------------------------- -class TestOffsets: - def test_resultant_offsets_applied(self): - mock = MockSensorClient(connected_sensors=["thumb"]) - mock.connect() - mock.set_mock_forces({"thumb": [5.0, 3.0, 10.0]}) - mock.set_taxel_offsets({"thumb": [[1.0, 0.5, 2.0]]}) - - # Resultant offset = sum of taxel offsets - result = mock.read_resultant_force() - assert result["thumb"][0] == pytest.approx(4.0, abs=0.1) # 5.0 - 1.0 - assert result["thumb"][1] == pytest.approx(2.5, abs=0.1) # 3.0 - 0.5 - assert result["thumb"][2] == pytest.approx(8.0, abs=0.1) # 10.0 - 2.0 - - def test_fz_clamped_to_zero(self): - """fz should never go negative after offset subtraction.""" - mock = MockSensorClient(connected_sensors=["thumb"]) - mock.connect() - mock.set_mock_forces({"thumb": [0.0, 0.0, 1.0]}) - mock.set_taxel_offsets({"thumb": [[0.0, 0.0, 5.0]]}) +@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["thumb"][2] == 0.0 # clamped, not -4.0 + result = mock.read_resultant_force() + assert result[finger] == [4.0, 2.5, 8.0] - def test_clear_offsets(self): - mock = MockSensorClient(connected_sensors=["thumb"]) - mock.connect() - 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] # No offset applied +@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]]}) - def test_stream_offsets_applied(self): - """Offsets should also apply to auto-stream data.""" - mock = MockSensorClient(connected_sensors=["thumb"]) - mock.connect() - mock.set_mock_forces({"thumb": [5.0, 3.0, 10.0]}) - mock.set_taxel_offsets({"thumb": [[1.0, 0.5, 2.0]]}) - mock.start_auto_stream(resultant=True, taxels=False) - result, _ = _poll_auto_latest(mock) - mock.stop_auto_stream() - mock.disconnect() + result = mock.read_resultant_force() + assert result[finger][2] == 0.0 - assert result["thumb"][0] == pytest.approx(4.0, abs=0.1) - assert result["thumb"][2] == pytest.approx(8.0, abs=0.1) - def test_taxel_offsets_applied_in_stream(self): - """Per-taxel offsets should apply to taxel auto-stream data.""" - 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 = MockSensorClient( - connected_sensors=["thumb"], - taxel_provider=lambda: {"thumb": [list(t) for t in taxels]}, - ) - mock.connect() - mock._sim_taxel_counts["thumb"] = 2 - mock._sensor_config = mock._get_configuration() - mock.set_taxel_offsets({"thumb": offsets}) - mock.start_auto_stream(resultant=False, taxels=True) - result, _ = _poll_auto_latest_taxels(mock) - mock.stop_auto_stream() - mock.disconnect() +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] + - assert result["thumb"][0][0] == pytest.approx(1.5, abs=0.1) # 2.0 - 0.5 - assert result["thumb"][0][2] == pytest.approx(4.0, abs=0.1) # 5.0 - 1.0 - assert result["thumb"][1][0] == pytest.approx(2.0, abs=0.1) # 3.0 - 1.0 - assert result["thumb"][1][2] == pytest.approx(6.0, abs=0.1) # 8.0 - 2.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 # --------------------------------------------------------------------------- -class TestErrorPaths: - def test_read_before_connect_raises(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - with pytest.raises(OSError, match="connect"): - mock.read_resultant_force() +def test_read_before_connect_raises(): + mock = MockSensorClient(connected_sensors=ALL_FINGERS) + with pytest.raises(OSError, match="connect"): + mock.read_resultant_force() - def test_get_auto_latest_before_stream_returns_none(self): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) - mock.connect() - result, ts = mock.get_auto_latest() - mock.disconnect() - assert result is None - assert ts is None +def test_get_auto_latest_before_stream_returns_none(mock): + result, ts = mock.get_auto_latest() + assert result is None + assert ts is None # --------------------------------------------------------------------------- # SensorConfiguration ordering # --------------------------------------------------------------------------- -class TestSensorConfigOrdering: - def test_slot_order_default(self): - config = _make_config(ALL_FINGERS) - assert config.active_sensors == ["thumb", "index", "middle", "ring", "pinky"] - - def test_slot_order_custom_mapping(self): - custom_map = {"thumb": 1, "index": 3, "middle": 0, "ring": 2, "pinky": 4} - config = _make_config(ALL_FINGERS, finger_to_sensor_id=custom_map) - # Sorted by sensor_id: middle(0), thumb(1), ring(2), index(3), pinky(4) - assert config.active_sensors == ["middle", "thumb", "ring", "index", "pinky"] - - def test_subset_preserves_order(self): - config = _make_config(["pinky", "thumb"]) - # thumb=0, pinky=4 → thumb first - assert config.active_sensors == ["thumb", "pinky"] +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"] From 8ee244b5b3b9ce332f0f5ffa4b83d5ae32433a90 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 5 May 2026 10:50:50 +0200 Subject: [PATCH 12/20] Fix resultant decoder to read low byte only Mock now round-trips through the real codec so wire-format bugs surface in tests. --- CLAUDE.md | 1 - orca_core/hardware/mock_sensor_client.py | 50 ++++++++++-- orca_core/hardware/sensing/constants.py | 2 +- orca_core/hardware/sensing/protocol.py | 96 +++++++++++++++++++----- tests/test_protocol.py | 65 +++++++++++++--- tests/test_tactile_sensor.py | 8 +- 6 files changed, 186 insertions(+), 36 deletions(-) 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/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index f0a91daf..236049f7 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -26,6 +26,12 @@ ForceVector, 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__) @@ -208,7 +214,18 @@ def _write_register(self, address: int, data: bytes, response_timeout_s: float = # ========================================================================= def _read_raw_resultant(self) -> ResultantForces: - return self._resultant_provider() + 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 SensorConfiguration.""" + 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) @@ -236,14 +253,37 @@ def _acquire_frame( parse_taxels: bool, min_sensors: int, ) -> tuple[dict | None, dict | None]: - """Acquire simulated frame data from mock providers.""" + """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. + """ if self._sensor_config is None or self._sensor_config.num_active_sensors < min_sensors: raise NoSensorsAvailableError("Insufficient sensors for auto-stream") - parsed_resultant = self._resultant_provider() if parse_resultant else None - parsed_taxels = self._taxel_provider() if parse_taxels else None + active = self._sensor_config.active_sensors + num_taxels = self._sensor_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 - # Rate limiting (None = no sleep, ideal for tests) if self._auto_rate_hz: time.sleep(1.0 / self._auto_rate_hz) diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py index ea9b78c6..214d523c 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -67,7 +67,7 @@ RESOLUTION_N_PER_LSB = 0.1 # Byte sizes per data element -BYTES_PER_RESULTANT = 6 # fx(int16) + fy(int16) + fz(uint16) +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 diff --git a/orca_core/hardware/sensing/protocol.py b/orca_core/hardware/sensing/protocol.py index 38dd22e4..8379f898 100644 --- a/orca_core/hardware/sensing/protocol.py +++ b/orca_core/hardware/sensing/protocol.py @@ -355,28 +355,27 @@ def _validate_payload_size(data: bytes, expected: int, context: str) -> None: ) -def _unpack_force_vector(data: bytes, offset: int, width: int) -> ForceVector: - """Unpack a force vector (fx signed, fy signed, fz unsigned) from packed bytes. - - Args: - data: Raw byte buffer - offset: Start position in data - width: Bytes per component (1 for taxels, 2 for resultants) - """ - fx = int.from_bytes(data[offset:offset + width], "little", signed=True) * RESOLUTION_N_PER_LSB - fy = int.from_bytes(data[offset + width:offset + 2 * width], "little", signed=True) * RESOLUTION_N_PER_LSB - fz = int.from_bytes(data[offset + 2 * width:offset + 3 * width], "little", signed=False) * RESOLUTION_N_PER_LSB +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, FORCE_DECIMAL_PLACES), round(fy, FORCE_DECIMAL_PLACES), round(fz, FORCE_DECIMAL_PLACES)] def _unpack_resultant(data: bytes, offset: int) -> ForceVector: - """Unpack one resultant force vector: fx(int16), fy(int16), fz(uint16).""" - return _unpack_force_vector(data, offset, width=2) + """Unpack one resultant force vector from a 6-byte module slot. - -def _unpack_taxel(data: bytes, offset: int) -> ForceVector: - """Unpack one taxel force vector: fx(int8), fy(int8), fz(uint8).""" - return _unpack_force_vector(data, offset, width=1) + 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, FORCE_DECIMAL_PLACES), round(fy, FORCE_DECIMAL_PLACES), round(fz, FORCE_DECIMAL_PLACES)] def decode_resultant_auto( @@ -640,3 +639,66 @@ 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: + """Encode resultant-only auto-stream payload for mock use.""" + 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: + """Encode taxel-only auto-stream payload for mock use.""" + 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: + """Encode interleaved (resultant + taxels) auto-stream payload for mock use.""" + 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/tests/test_protocol.py b/tests/test_protocol.py index ef13eee7..ba110bc5 100644 --- a/tests/test_protocol.py +++ b/tests/test_protocol.py @@ -26,6 +26,8 @@ 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 ( PROTOCOL_HEADER_REQUEST, @@ -328,24 +330,43 @@ def test_compute_combined_payload_size(): # --------------------------------------------------------------------------- # Payload decoders — auto-stream # -# Wire format reminder: -# Resultant per sensor: , little-endian, ÷10 → N -# Taxel per element: , ÷10 → N +# 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): - data = b"".join(struct.pack(" 127, firmware sign-extends + struct.pack("bbB", 0, -10, 20) ) forces, taxels = decode_combined_auto( @@ -389,6 +410,32 @@ def test_decode_combined_auto_interleaved(): 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 # --------------------------------------------------------------------------- @@ -396,8 +443,8 @@ def test_decode_combined_auto_interleaved(): def test_decode_resultant_register_known_values(): # Module index for thumb (slot 0): 0*4+2 = 2, byte offset = 12 data = bytearray(168) - struct.pack_into(" 127 + result = decode_resultant_register(bytes(data), ["thumb"], {"thumb": 2}) assert result["thumb"] == [10.0, -5.0, 20.0] diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index c9143383..a2855ce1 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -149,9 +149,10 @@ def test_combined_mode_returns_both_streams(mock): ) def test_custom_provider_is_used(kind): if kind == "resultant": + marker = [4.2, -3.0, 20.0] mock = MockSensorClient( connected_sensors=["thumb"], - resultant_provider=lambda: {"thumb": [42.0, 0.0, 0.0]}, + resultant_provider=lambda: {"thumb": marker}, ) mock.connect() mock.start_auto_stream(resultant=True, taxels=False) @@ -159,11 +160,12 @@ def test_custom_provider_is_used(kind): result, _ = mock.get_auto_latest() mock.stop_auto_stream() mock.disconnect() - assert result["thumb"] == [42.0, 0.0, 0.0] + assert result["thumb"] == marker else: - marker = [[99.0, 88.0, 77.0]] + marker = [[9.9, -8.8, 7.7]] mock = MockSensorClient( connected_sensors=["thumb"], + taxel_counts={"thumb": 1}, taxel_provider=lambda: {"thumb": marker}, ) mock.connect() From af7a917f98f82c36f32e9f85b5256a5e62ad0b43 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 5 May 2026 12:13:05 +0200 Subject: [PATCH 13/20] Add tactile sensor port cascade and bring-up helpers OrcaHandTouch.connect() now mirrors the motor cascade: configured port first, then USB-VID auto-detection, with a successful auto-detect persisted to config.yaml. Adds connect_sensors_only() for motor-less sensor testing and makes get_auto_stats() return a snapshot copy so callers can diff values across calls. --- orca_core/constants.py | 3 ++ orca_core/hardware/sensor_client.py | 6 ++- orca_core/hardware_hand.py | 67 ++++++++++++++++++++++++++--- 3 files changed, 68 insertions(+), 8 deletions(-) diff --git a/orca_core/constants.py b/orca_core/constants.py index 5f51d363..629811d3 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 + ], } """ diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index 95529cc5..8887383f 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -7,6 +7,7 @@ # ============================================================================== from __future__ import annotations +import dataclasses from dataclasses import dataclass, field import serial import threading @@ -683,11 +684,14 @@ def get_auto_latest_all(self): def get_auto_stats(self): """Thread-safe snapshot of auto-stream diagnostics. + Returns a copy so callers can compare values across calls without + racing the reader thread. + See `AutoStreamStats` for the meaning of each field. Useful for health monitoring (frame rate, checksum errors, reconfigurations). """ with self._auto_lock: - return self._auto_stats + return dataclasses.replace(self._auto_stats) def set_taxel_offsets(self, offsets: dict) -> None: """Set per-taxel zeroing offsets and compute resultant offsets. diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index ae8cb54d..fefd4234 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -21,7 +21,7 @@ from .hand_config import OrcaHandConfig, OrcaHandTouchConfig from .hardware.motor_client import MotorClient from .hardware.sensing.types import ResultantReading, TaxelReading -from .utils.utils import auto_detect_port, get_and_choose_port, update_yaml +from .utils.utils import auto_detect_port, get_and_choose_port, read_yaml, update_yaml if TYPE_CHECKING: from .hardware.dynamixel_client import DynamixelClient @@ -1179,19 +1179,64 @@ def _create_sensor_client(self): finger_to_sensor_id=self.config.finger_to_sensor_id, ) - def connect(self) -> tuple[bool, str]: - success, msg = super().connect() - if not success: - return success, msg + 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._sensor_client = self._create_sensor_client() try: self._sensor_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._sensor_client = None - return False, f"{msg} | Sensor connection failed: {e}" - return True, f"{msg} | Sensor connected" + 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._sensor_client = self._create_sensor_client() + self._sensor_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._sensor_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._sensor_client is not None and self._sensor_client.is_connected: @@ -1251,6 +1296,14 @@ def clear_tactile_zero(self) -> None: def get_sensor_configuration(self): return self._sensor_client.get_sensor_configuration() + def get_tactile_stats(self): + """Return ``AutoStreamStats`` for the running auto-stream. + + Useful for monitoring stream health (``frames_ok``, ``frames_bad_checksum``, + ``parse_errors``, ``resyncs``, ``reconfiguration_count``, ``last_error_code``). + """ + return self._sensor_client.get_auto_stats() + class MockOrcaHand(OrcaHand): """Drop-in :class:`OrcaHand` backed by an in-memory mock motor client, From 2d2ae9c8bbc966462d2b4e21b09e6d05a346553c Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 5 May 2026 12:13:20 +0200 Subject: [PATCH 14/20] Add tactile sensor health-check script --- scripts/test_sensors.py | 528 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 528 insertions(+) create mode 100644 scripts/test_sensors.py diff --git a/scripts/test_sensors.py b/scripts/test_sensors.py new file mode 100644 index 00000000..791b5ab0 --- /dev/null +++ b/scripts/test_sensors.py @@ -0,0 +1,528 @@ +#!/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 + +# All thresholds, taxel layouts, and role mappings below live here rather +# than in orca_core.constants because they are only used by this health- +# check script — to decide pass/fail and to render a barebones ASCII view. +# They are not part of the OrcaHandTouch API and have no meaning at +# runtime for downstream code. + +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 finger_with_largest_offset(taxel_offsets): + """Return (finger, total_|fz|) with the largest cumulative fz baseline, + i.e. the finger whose sensor had the most non-zero rest signal.""" + best, best_total = None, -1.0 + for finger, taxels in taxel_offsets.items(): + total = sum(abs(t[2]) for t in taxels) + if total > best_total: + best, best_total = finger, total + return best, best_total + + +def phase_1_enumerate(hand): + banner(1, "Connect & enumerate") + cfg = hand.get_sensor_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_sensor_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_sensor_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, max |fz|):") + 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) + peak = max(fz_vals) + print(f" {f:7s} avg={avg:.3f} N max={peak:.2f} 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})" + + target_finger, target_total = finger_with_largest_offset(offsets) + print(f" Highest-baseline finger was '{target_finger}' (total |fz|={target_total:.2f} N)") + pause(f"press {target_finger.upper()} firmly") + peak = measure_all_peaks(hand.get_tactile_forces, FINGERS, duration_s=1.5)[target_finger] + print(f" Peak |fz| on {target_finger} after zero: {peak:.2f} N") + if peak < PRESS_THRESHOLD_N: + return False, f"press not detected on {target_finger} after zero: peak={peak:.2f} N" + + 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), {target_finger} press detected, 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() From 111bb38cf5a34d60475af7747c5b5eaaf780ee87 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Tue, 5 May 2026 12:39:00 +0200 Subject: [PATCH 15/20] Remove dead reconfiguration code and simplify health-check zeroing --- orca_core/hardware/mock_sensor_client.py | 39 +------ orca_core/hardware/sensor_client.py | 132 ++++------------------- orca_core/hardware_hand.py | 2 +- scripts/test_sensors.py | 26 +---- tests/test_tactile_sensor.py | 43 +------- 5 files changed, 28 insertions(+), 214 deletions(-) diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_sensor_client.py index 236049f7..c654af10 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_sensor_client.py @@ -14,7 +14,7 @@ import time import logging -from orca_core.hardware.sensor_client import SensorClient, NoSensorsAvailableError +from orca_core.hardware.sensor_client import SensorClient from orca_core.hardware.sensing.constants import ( FINGER_NAMES, DEFAULT_TAXEL_COUNTS, @@ -50,7 +50,6 @@ class MockSensorClient(SensorClient): Control simulated data via: - set_mock_forces(): Set specific force values to return - set_mock_taxels(): Set specific taxel values to return - - set_connected_sensors(): Configure which sensors appear connected - set_resultant_provider()/set_taxel_provider(): Inject deterministic generators Default behavior (if no providers or mock values are set): @@ -73,15 +72,13 @@ def __init__( if connected_sensors is None: connected_sensors = list(FINGER_NAMES) - self._taxel_counts_per_finger: dict[str, int] = ( - dict(taxel_counts) if taxel_counts is not None else dict(DEFAULT_TAXEL_COUNTS) - ) + 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: self._taxel_counts_per_finger[f] if f in connected_sensors else 0 + f: full_taxel_counts[f] if f in connected_sensors else 0 for f in FINGER_NAMES } @@ -107,22 +104,6 @@ def __init__( # Mock Control Methods # ========================================================================= - def set_connected_sensors(self, sensors: list[str]) -> None: - """Configure which sensors appear as connected. - - Only clears mock data for sensors that were removed. - """ - removed = {f for f, on in self._sim_connected.items() if on and f not in sensors} - self._update_connectivity(sensors) - for f in removed: - self._mock_forces.pop(f, None) - self._mock_taxels.pop(f, None) - - def simulate_dropout(self, dropped: list[str]) -> None: - """Simulate one or more sensors dropping out.""" - remaining = [f for f in self._sim_connected if self._sim_connected[f] and f not in dropped] - self.set_connected_sensors(remaining) - def set_mock_forces(self, forces: ResultantForces) -> None: """Set the force values to return for each sensor. @@ -251,7 +232,6 @@ def _acquire_frame( self, parse_resultant: bool, parse_taxels: bool, - min_sensors: int, ) -> tuple[dict | None, dict | None]: """Acquire simulated frame data, round-tripped through the real wire codec. @@ -259,9 +239,6 @@ def _acquire_frame( ensures mock-driven tests exercise the actual protocol decoders instead of bypassing them. """ - if self._sensor_config is None or self._sensor_config.num_active_sensors < min_sensors: - raise NoSensorsAvailableError("Insufficient sensors for auto-stream") - active = self._sensor_config.active_sensors num_taxels = self._sensor_config.num_taxels @@ -293,16 +270,6 @@ def _acquire_frame( # Internal Helpers # ========================================================================= - def _update_connectivity(self, sensors: list[str]) -> None: - """Update simulated connectivity and reconfigure if connected.""" - self._sim_connected = {f: f in sensors for f in FINGER_NAMES} - self._sim_taxel_counts = { - f: self._taxel_counts_per_finger[f] if self._sim_connected[f] else 0 - for f in FINGER_NAMES - } - if self._connected: - self._sensor_config = self._get_configuration() - def _default_resultant_provider(self) -> ResultantForces: forces = self._mock_forces return { diff --git a/orca_core/hardware/sensor_client.py b/orca_core/hardware/sensor_client.py index 8887383f..747bb73e 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/sensor_client.py @@ -44,9 +44,6 @@ read_response_body_size, extract_auto_frame_eff_len, unpack_auto_payload, - compute_resultant_payload_size, - compute_taxel_payload_size, - compute_combined_payload_size, compute_expected_payload_size, compute_distal_module_index, decode_resultant_auto, @@ -88,15 +85,12 @@ class AutoStreamStats: frames_bad_checksum: Frames rejected due to checksum (LRC) mismatch. parse_errors: Frames received intact but whose payload failed to decode. resyncs: Times the reader had to resync after IO errors or bad framing. - reconfiguration_count: Times the sensor configuration was re-queried - after a payload-size mismatch. last_error_code: Most recent sensor-reported error code (0 = no error). """ frames_ok: int = 0 frames_bad_checksum: int = 0 parse_errors: int = 0 resyncs: int = 0 - reconfiguration_count: int = 0 last_error_code: int = 0 @@ -104,17 +98,13 @@ class AutoStreamStats: class SensorConfiguration: """Snapshot of connected sensors and their properties. - This configuration is captured when connecting or when errors trigger - reconfiguration. It's used to build dynamic parsers that adapt to - available sensors. + 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} - expected_payload_size_resultant: int = 0 # Expected bytes for resultant force mode - expected_payload_size_taxels: int = 0 # Expected bytes for taxel mode - expected_payload_size_combined: int = 0 # Expected bytes for resultant + taxel mode - timestamp: float = 0.0 # When this config was captured finger_to_sensor_id: dict[str, int] = field(default_factory=lambda: dict(DEFAULT_FINGER_TO_SENSOR_ID)) @property @@ -513,10 +503,9 @@ def get_sensor_configuration(self) -> SensorConfiguration | None: return self._sensor_config def _get_configuration(self) -> SensorConfiguration: - """Snapshot the current sensor configuration. + """Snapshot the current sensor configuration at stream start. Reads connected sensors and their properties from the hardware. - This is called on connect and when errors trigger reconfiguration. Returns: SensorConfiguration with current hardware state @@ -528,27 +517,16 @@ def _get_configuration(self) -> SensorConfiguration: connected = self.read_connected_sensors() num_taxels = self.read_num_taxels() - # Build module indices for active sensors (fingertip only) 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) - # Calculate expected payload sizes - active = [f for f in FINGER_NAMES if connected.get(f, False)] - expected_resultant = compute_resultant_payload_size(len(active)) - expected_taxels = compute_taxel_payload_size(active, num_taxels) - expected_combined = compute_combined_payload_size(active, num_taxels) - config = SensorConfiguration( connected=connected, num_taxels=num_taxels, module_indices=module_indices, - expected_payload_size_resultant=expected_resultant, - expected_payload_size_taxels=expected_taxels, - expected_payload_size_combined=expected_combined, - timestamp=time.time(), finger_to_sensor_id=dict(self._finger_to_sensor_id), ) @@ -559,49 +537,6 @@ def _get_configuration(self) -> SensorConfiguration: logger.error(f"Failed to get sensor configuration: {e}") raise IOError(f"Failed to read sensor configuration: {e}") from e - def _reconfigure(self) -> bool: - """Re-query the sensor board and update the cached configuration. - - Called by `_acquire_frame` when a payload-size mismatch indicates the - hardware reconfigured itself (sensor connected or disconnected). - - Returns: - True if the active-sensor set changed, False if unchanged. - - Raises: - NoSensorsAvailableError: If no sensors are connected after reconfiguration. - """ - logger.info("Attempting reconfiguration...") - new_config = self._get_configuration() - - if self._sensor_config is not None: - old_active = set(self._sensor_config.active_sensors) - new_active = set(new_config.active_sensors) - - if old_active == new_active: - logger.debug("Configuration unchanged, no reconfiguration needed") - return False - - added = new_active - old_active - removed = old_active - new_active - if added: - logger.info(f"Sensors added: {', '.join(added)}") - if removed: - logger.warning(f"Sensors removed: {', '.join(removed)}") - - self._sensor_config = new_config - - with self._auto_lock: - self._auto_stats.reconfiguration_count += 1 - - if new_config.num_active_sensors == 0: - logger.error("No sensors available after reconfiguration") - raise NoSensorsAvailableError("All sensors disconnected") - - logger.info(f"Reconfiguration successful: {new_config}") - return True - - def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> None: """Configure which data types to include in auto stream. @@ -688,7 +623,7 @@ def get_auto_stats(self): racing the reader thread. See `AutoStreamStats` for the meaning of each field. Useful for health - monitoring (frame rate, checksum errors, reconfigurations). + monitoring (frame rate, checksum errors, parse errors). """ with self._auto_lock: return dataclasses.replace(self._auto_stats) @@ -883,12 +818,11 @@ def _acquire_frame( self, parse_resultant: bool, parse_taxels: bool, - min_sensors: int, ) -> tuple[dict | None, dict | None]: """Acquire and return the next parsed (resultant, taxels) frame. - Reads one auto-stream frame from serial, validates LRC, handles payload - size mismatches with reconfiguration, and parses the data. + Reads one auto-stream frame from serial, validates LRC, and parses + the data. Subclasses (e.g. MockSensorClient) override this to provide data from other sources while inheriting the loop's stats, offset, and lifecycle logic. @@ -899,49 +833,29 @@ def _acquire_frame( Raises: FrameError: Recoverable frame-level error (bad LRC, parse failure) - NoSensorsAvailableError: No sensors available after reconfiguration IOError: Serial communication failure or auto stream stopped """ - # Find and consume AA 56 header self._resync_to_auto_header() - # Read frame metadata, payload, and checksum 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] - # Debug print (throttled to once per second) now = time.time() if now - self._last_frame_debug_print > 1.0: config_str = str(self._sensor_config) if self._sensor_config else "no config" logger.debug(f"[auto] eff_len={eff_len}, config={config_str}") self._last_frame_debug_print = now - # Validate frame integrity if not validate_auto_frame_lrc(meta, payload, lrc): raise FrameError("LRC mismatch", bad_lrc=True) - # Split error code and force data err_code, valid = unpack_auto_payload(payload) - # Update serial-specific stats with self._auto_lock: self._auto_stats.last_error_code = err_code - # Check for payload size mismatch (indicates config change) - if self._sensor_config: - expected_size = self._get_expected_payload_size(self._sensor_config) - if len(valid) != expected_size and expected_size > 0: - logger.warning( - f"Payload size mismatch: expected {expected_size}, got {len(valid)}. " - "Triggering reconfiguration..." - ) - if self._reconfigure(): - logger.info("Reconfiguration successful, continuing stream") - raise FrameError("Payload size mismatch, skipping frame") - - # Parse payload based on mode if not self._sensor_config: raise FrameError("No sensor configuration available") @@ -970,7 +884,7 @@ def _acquire_frame( return parsed_resultant, parsed_taxels - def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_sensors: int): + def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): """Background thread that continuously reads and parses auto-stream frames. Calls _acquire_frame() to get parsed data, then applies offsets and updates @@ -978,23 +892,15 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso inheriting the shared loop logic. Error handling: - - FrameError: recoverable (bad LRC / parse error / size mismatch). Count - and continue. Size-mismatch frames trigger reconfiguration inside - _acquire_frame. - - NoSensorsAvailableError: terminal. Stop the stream cleanly. + - FrameError: recoverable (bad LRC / parse error). Count and continue. - IOError: serial-level hiccup. Count, back off briefly, continue. - Any other Exception: unexpected (likely a programming bug). Log the traceback and stop the stream loudly rather than rotting silently. - - Args: - parse_resultant: Whether to parse resultant force data - parse_taxels: Whether to parse individual taxel data - min_sensors: Minimum number of sensors required to continue streaming """ while self._auto_running.is_set(): try: parsed_resultant, parsed_taxels = self._acquire_frame( - parse_resultant, parse_taxels, min_sensors + parse_resultant, parse_taxels ) self._apply_stream_offsets(parsed_resultant, parsed_taxels) @@ -1015,11 +921,6 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool, min_senso else: self._auto_stats.parse_errors += 1 - except NoSensorsAvailableError: - logger.error("No sensors available, stopping stream") - self._auto_running.clear() - break - except IOError as e: if "Auto stream stopped" in str(e): logger.info("Auto stream stopped") @@ -1048,7 +949,11 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se - get_auto_latest_taxels(): for taxel data - get_auto_latest_all(): for both - The system automatically adapts to sensor configuration changes (connects/disconnects). + Sensor presence is determined at stream start. Paxini PX-6AX GEN3 + firmware enumerates sensors at power-on and does not report mid-stream + disconnects — a physically disconnected sensor's slot continues to + emit its last bytes until power-cycle. Restart the host process and + power-cycle the board to pick up wiring changes. Setup sequence: 1. Stop any existing stream @@ -1061,8 +966,9 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se Args: resultant: Include resultant force data (fx, fy, fz per sensor) taxels: Include individual taxel force data - min_sensors: Minimum number of sensors required (default: 1) - If fewer sensors available, raises NoSensorsAvailableError + min_sensors: Minimum number of sensors required at stream start + (default: 1). If fewer sensors are enumerated, + raises NoSensorsAvailableError. Raises: OSError: If not connected to sensor @@ -1127,7 +1033,7 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se self._auto_running.set() # Signal thread to run self._auto_thread = threading.Thread( target=self._auto_reader_loop, - args=(resultant, taxels, min_sensors), + args=(resultant, taxels), daemon=True # Thread exits when main program exits ) self._auto_thread.start() diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index fefd4234..90e46055 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1300,7 +1300,7 @@ def get_tactile_stats(self): """Return ``AutoStreamStats`` for the running auto-stream. Useful for monitoring stream health (``frames_ok``, ``frames_bad_checksum``, - ``parse_errors``, ``resyncs``, ``reconfiguration_count``, ``last_error_code``). + ``parse_errors``, ``resyncs``, ``last_error_code``). """ return self._sensor_client.get_auto_stats() diff --git a/scripts/test_sensors.py b/scripts/test_sensors.py index 791b5ab0..01316588 100644 --- a/scripts/test_sensors.py +++ b/scripts/test_sensors.py @@ -236,17 +236,6 @@ def detect_wiring_mismatch(target_finger, peaks, wiring, threshold=PRESS_THRESHO ) -def finger_with_largest_offset(taxel_offsets): - """Return (finger, total_|fz|) with the largest cumulative fz baseline, - i.e. the finger whose sensor had the most non-zero rest signal.""" - best, best_total = None, -1.0 - for finger, taxels in taxel_offsets.items(): - total = sum(abs(t[2]) for t in taxels) - if total > best_total: - best, best_total = finger, total - return best, best_total - - def phase_1_enumerate(hand): banner(1, "Connect & enumerate") cfg = hand.get_sensor_configuration() @@ -394,13 +383,12 @@ def phase_5_zeroing(hand): 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, max |fz|):") + 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) - peak = max(fz_vals) - print(f" {f:7s} avg={avg:.3f} N max={peak:.2f} N ({len(fz_vals)} taxels)") + print(f" {f:7s} avg={avg:.3f} N ({len(fz_vals)} taxels)") time.sleep(0.2) forces = hand.get_tactile_forces() @@ -409,21 +397,13 @@ def phase_5_zeroing(hand): if max_resting is None or max_resting > ZERO_TOLERANCE_N: return False, f"resting fz not near zero (max={max_resting})" - target_finger, target_total = finger_with_largest_offset(offsets) - print(f" Highest-baseline finger was '{target_finger}' (total |fz|={target_total:.2f} N)") - pause(f"press {target_finger.upper()} firmly") - peak = measure_all_peaks(hand.get_tactile_forces, FINGERS, duration_s=1.5)[target_finger] - print(f" Peak |fz| on {target_finger} after zero: {peak:.2f} N") - if peak < PRESS_THRESHOLD_N: - return False, f"press not detected on {target_finger} after zero: peak={peak:.2f} N" - 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), {target_finger} press detected, cleared" + return True, f"zero captured, applied (max resting={max_resting:.2f}N), cleared" finally: hand.stop_tactile_stream() diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index a2855ce1..69ce8568 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -1,21 +1,14 @@ """Tests for MockSensorClient integration and SensorConfiguration contracts. Validates the mock's lifecycle (connect → stream → read → stop), offset logic, -dynamic reconfiguration, and configuration ordering. Pure protocol codec tests -live in test_protocol.py. +and configuration ordering. Pure protocol codec tests live in test_protocol.py. """ -import time - import pytest from orca_core.hardware.sensor_client import SensorConfiguration from orca_core.hardware.mock_sensor_client import MockSensorClient -from orca_core.hardware.sensing.constants import ( - DEFAULT_TAXEL_COUNTS, - BYTES_PER_RESULTANT, - BYTES_PER_TAXEL, -) +from orca_core.hardware.sensing.constants import DEFAULT_TAXEL_COUNTS from orca_core.hardware.sensing.protocol import compute_distal_module_index ALL_FINGERS = ["thumb", "index", "middle", "ring", "pinky"] @@ -60,18 +53,10 @@ def _make_config( 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} - num_active = len(connected_fingers) - expected_resultant = num_active * BYTES_PER_RESULTANT - expected_taxels = sum(num_taxels[f] * BYTES_PER_TAXEL for f in connected_fingers) - return SensorConfiguration( connected=connected, num_taxels=num_taxels, module_indices=module_indices, - expected_payload_size_resultant=expected_resultant, - expected_payload_size_taxels=expected_taxels, - expected_payload_size_combined=expected_resultant + expected_taxels, - timestamp=time.time(), finger_to_sensor_id=finger_to_sensor_id, ) @@ -177,30 +162,6 @@ def test_custom_provider_is_used(kind): assert result["thumb"] == marker -# --------------------------------------------------------------------------- -# Dynamic reconfiguration -# --------------------------------------------------------------------------- - -def test_simulate_dropout_removes_sensors(mock): - assert mock._sensor_config.num_active_sensors == 5 - mock.simulate_dropout(["index", "ring"]) - assert mock._sensor_config.num_active_sensors == 3 - assert "index" not in mock._sensor_config.active_sensors - assert "ring" not in mock._sensor_config.active_sensors - - -def test_set_connected_sensors_updates_config(mock): - mock.set_connected_sensors(["thumb"]) - assert mock._sensor_config.active_sensors == ["thumb"] - assert mock._sensor_config.num_active_sensors == 1 - - -def test_dropout_clears_mock_data(mock): - mock.set_mock_forces({"index": [5.0, 0.0, 0.0]}) - mock.simulate_dropout(["index"]) - assert "index" not in mock._mock_forces - - # --------------------------------------------------------------------------- # Offset logic # --------------------------------------------------------------------------- From 537d36785802d02e999e03e96195973ffc19a573 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Wed, 6 May 2026 14:59:24 +0200 Subject: [PATCH 16/20] Rename SensorClient to TactileClient and add combined-data accessor --- ...ensor_client.py => mock_tactile_client.py} | 16 ++-- orca_core/hardware/sensing/constants.py | 2 +- orca_core/hardware/sensing/types.py | 14 ++++ .../{sensor_client.py => tactile_client.py} | 52 ++++++------- orca_core/hardware_hand.py | 73 +++++++++++-------- scripts/test_sensors.py | 6 +- tests/test_tactile_sensor.py | 24 +++--- 7 files changed, 108 insertions(+), 79 deletions(-) rename orca_core/hardware/{mock_sensor_client.py => mock_tactile_client.py} (96%) rename orca_core/hardware/{sensor_client.py => tactile_client.py} (95%) diff --git a/orca_core/hardware/mock_sensor_client.py b/orca_core/hardware/mock_tactile_client.py similarity index 96% rename from orca_core/hardware/mock_sensor_client.py rename to orca_core/hardware/mock_tactile_client.py index c654af10..4bc9e1a9 100644 --- a/orca_core/hardware/mock_sensor_client.py +++ b/orca_core/hardware/mock_tactile_client.py @@ -14,7 +14,7 @@ import time import logging -from orca_core.hardware.sensor_client import SensorClient +from orca_core.hardware.tactile_client import TactileClient from orca_core.hardware.sensing.constants import ( FINGER_NAMES, DEFAULT_TAXEL_COUNTS, @@ -40,10 +40,10 @@ TaxelProvider = Callable[[], TaxelForces] -class MockSensorClient(SensorClient): +class MockTactileClient(TactileClient): """Mock client for simulating tactile sensor communication in tests. - Subclasses SensorClient, replacing hardware I/O with deterministic data + 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. @@ -144,8 +144,8 @@ def connect(self) -> None: if self.is_connected: return self._connected = True - self._sensor_config = self._get_configuration() - logger.info(f"[MOCK] Connected, config: {self._sensor_config}") + self._tactile_config = self._get_configuration() + logger.info(f"[MOCK] Connected, config: {self._tactile_config}") def disconnect(self) -> None: if not self.is_connected: @@ -202,7 +202,7 @@ def _read_raw_resultant(self) -> ResultantForces: ) def _active_in_slot_order(self, forces: dict) -> list[str]: - """Sort the provider's fingers by hardware slot ID, matching SensorConfiguration.""" + """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)), @@ -239,8 +239,8 @@ def _acquire_frame( ensures mock-driven tests exercise the actual protocol decoders instead of bypassing them. """ - active = self._sensor_config.active_sensors - num_taxels = self._sensor_config.num_taxels + active = self._tactile_config.active_sensors + num_taxels = self._tactile_config.num_taxels if parse_resultant and parse_taxels: forces = self._resultant_provider() diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py index 214d523c..607fdd26 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -38,7 +38,7 @@ FUNC_CODE_WRITE = 0x10 # --------------------------------------------------------------------------- -# Register addresses (used by sensor_client for read/write targets) +# Register addresses (used by tactile_client for read/write targets) # --------------------------------------------------------------------------- ADDR_RESET = 0x0022 diff --git a/orca_core/hardware/sensing/types.py b/orca_core/hardware/sensing/types.py index 7ca0d26f..a733f5e7 100644 --- a/orca_core/hardware/sensing/types.py +++ b/orca_core/hardware/sensing/types.py @@ -56,3 +56,17 @@ def fingers(self) -> list[str]: def as_array(self, finger: str) -> 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/sensor_client.py b/orca_core/hardware/tactile_client.py similarity index 95% rename from orca_core/hardware/sensor_client.py rename to orca_core/hardware/tactile_client.py index 747bb73e..989fc0d7 100644 --- a/orca_core/hardware/sensor_client.py +++ b/orca_core/hardware/tactile_client.py @@ -95,7 +95,7 @@ class AutoStreamStats: @dataclass -class SensorConfiguration: +class TactileSensorConfiguration: """Snapshot of connected sensors and their properties. Captured at stream start. The Paxini PX-6AX GEN3 firmware enumerates @@ -129,7 +129,7 @@ def __str__(self) -> str: return f"SensorConfig({self.num_active_sensors} active: {active})" -class SensorClient: +class TactileClient: """Client for communicating with ORCA Tactile Sensors""" def __init__(self, @@ -162,7 +162,7 @@ def __init__(self, self._sensor_id_to_finger = {v: k for k, v in self._finger_to_sensor_id.items()} # Sensor configuration (dynamic, adapts to connected sensors) - self._sensor_config: SensorConfiguration | None = None + self._tactile_config: TactileSensorConfiguration | None = None self._auto_thread: threading.Thread | None = None self._auto_running = threading.Event() # Thread-safe flag for auto stream @@ -211,8 +211,8 @@ def connect(self): # Get initial sensor configuration try: - self._sensor_config = self._get_configuration() - logger.info(f"Initial configuration: {self._sensor_config}") + 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}") # Don't fail connection, config will be retrieved when starting auto-stream @@ -451,7 +451,7 @@ def read_auto_data_type(self) -> dict: def _read_raw_resultant(self) -> dict[str, list[float]]: """Read raw resultant forces from hardware (no offset application). - MockSensorClient overrides this to return simulated + MockTactileClient overrides this to return simulated data. The public read_resultant_force() method calls this, then applies zeroing offsets. @@ -459,9 +459,9 @@ def _read_raw_resultant(self) -> dict[str, list[float]]: Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons """ # Ensure we have current configuration - if self._sensor_config is None: + if self._tactile_config is None: try: - self._sensor_config = self._get_configuration() + self._tactile_config = self._get_configuration() except IOError as e: logger.error(f"Failed to get configuration: {e}") # Fall back to static parsing using default module indices @@ -471,7 +471,7 @@ def _read_raw_resultant(self) -> dict[str, list[float]]: data = self._read_register(ADDR_RESULTANT_FORCE_START, RESULTANT_BLOCK_SIZE) return decode_resultant_register( - data, self._sensor_config.active_sensors, self._sensor_config.module_indices, + data, self._tactile_config.active_sensors, self._tactile_config.module_indices, ) def read_resultant_force(self) -> dict[str, list[float]]: @@ -494,21 +494,21 @@ def read_resultant_force(self) -> dict[str, list[float]]: self._apply_resultant_offsets(result) return result - def get_sensor_configuration(self) -> SensorConfiguration | None: + def get_tactile_configuration(self) -> TactileSensorConfiguration | None: """Get the current sensor configuration snapshot. Returns: - SensorConfiguration object with current sensor state, or None if not yet configured + TactileSensorConfiguration object with current sensor state, or None if not yet configured """ - return self._sensor_config + return self._tactile_config - def _get_configuration(self) -> SensorConfiguration: + def _get_configuration(self) -> TactileSensorConfiguration: """Snapshot the current sensor configuration at stream start. Reads connected sensors and their properties from the hardware. Returns: - SensorConfiguration with current hardware state + TactileSensorConfiguration with current hardware state Raises: IOError: If unable to read configuration from sensor @@ -523,7 +523,7 @@ def _get_configuration(self) -> SensorConfiguration: sensor_id = self._finger_to_sensor_id[finger] module_indices[finger] = compute_distal_module_index(sensor_id) - config = SensorConfiguration( + config = TactileSensorConfiguration( connected=connected, num_taxels=num_taxels, module_indices=module_indices, @@ -805,7 +805,7 @@ def _resync_to_auto_header(self) -> None: # If we exit the loop, auto stream was stopped raise IOError("Auto stream stopped during resync") - def _get_expected_payload_size(self, config: SensorConfiguration) -> int: + 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, @@ -824,7 +824,7 @@ def _acquire_frame( Reads one auto-stream frame from serial, validates LRC, and parses the data. - Subclasses (e.g. MockSensorClient) override this to provide data from + Subclasses (e.g. MockTactileClient) override this to provide data from other sources while inheriting the loop's stats, offset, and lifecycle logic. Returns: @@ -844,7 +844,7 @@ def _acquire_frame( now = time.time() if now - self._last_frame_debug_print > 1.0: - config_str = str(self._sensor_config) if self._sensor_config else "no config" + 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 @@ -856,16 +856,16 @@ def _acquire_frame( with self._auto_lock: self._auto_stats.last_error_code = err_code - if not self._sensor_config: + if not self._tactile_config: raise FrameError("No sensor configuration available") - expected_size = self._get_expected_payload_size(self._sensor_config) + 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._sensor_config + cfg = self._tactile_config if parse_resultant and parse_taxels: parsed_resultant, parsed_taxels = decode_combined_auto( valid, cfg.active_sensors, cfg.num_taxels, @@ -990,26 +990,26 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se # Get initial sensor configuration try: - self._sensor_config = self._get_configuration() + self._tactile_config = self._get_configuration() except IOError as e: raise OSError(f"Failed to get sensor configuration: {e}") from e # Check minimum sensor requirement - if self._sensor_config.num_active_sensors < min_sensors: + if self._tactile_config.num_active_sensors < min_sensors: raise NoSensorsAvailableError( - f"Only {self._sensor_config.num_active_sensors} sensor(s) available, " + f"Only {self._tactile_config.num_active_sensors} sensor(s) available, " f"need at least {min_sensors}" ) # Log expected payload size for debugging - expected_size = self._get_expected_payload_size(self._sensor_config) + 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._sensor_config}, " + f"Starting auto-stream with {self._tactile_config}, " f"mode={'+'.join(mode_str)}, expected_payload={expected_size} bytes" ) diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 90e46055..3a1be452 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -19,8 +19,10 @@ from .base_hand import BaseHand from .calibration import CalibrationResult from .hand_config import OrcaHandConfig, OrcaHandTouchConfig +from .hardware.mock_tactile_client import MockTactileClient from .hardware.motor_client import MotorClient -from .hardware.sensing.types import ResultantReading, TaxelReading +from .hardware.sensing.types import ResultantReading, TactileReading, TaxelReading +from .hardware.tactile_client import TactileClient from .utils.utils import auto_detect_port, get_and_choose_port, read_yaml, update_yaml if TYPE_CHECKING: @@ -1168,12 +1170,10 @@ def __init__( model_name=model_name, config=config, ) - self._sensor_client = None + self._tactile_client = None - def _create_sensor_client(self): - from .hardware.sensor_client import SensorClient - - return SensorClient( + 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, @@ -1194,25 +1194,25 @@ def _connect_sensor_with_fallback(self) -> tuple[bool, str]: (``KNOWN_VIDS["tactile_sensor"]``). On a successful auto-detect the new port is written back to ``config.yaml``. """ - self._sensor_client = self._create_sensor_client() + self._tactile_client = self._create_tactile_client() try: - self._sensor_client.connect() + 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._sensor_client = None + 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._sensor_client = self._create_sensor_client() - self._sensor_client.connect() + 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._sensor_client = None + self._tactile_client = None return False, ( "Sensor connection failed: no usable port (set sensors.port in config.yaml " @@ -1239,13 +1239,13 @@ def connect_sensors_only(self) -> tuple[bool, str]: return self._connect_sensor_with_fallback() def disconnect(self) -> None: - if self._sensor_client is not None and self._sensor_client.is_connected: + if self._tactile_client is not None and self._tactile_client.is_connected: try: - self._sensor_client.stop_auto_stream() + self._tactile_client.stop_auto_stream() except Exception: pass - self._sensor_client.disconnect() - self._sensor_client = None + self._tactile_client.disconnect() + self._tactile_client = None super().disconnect() def get_tactile_forces(self) -> ResultantReading | None: @@ -1257,7 +1257,7 @@ def get_tactile_forces(self) -> ResultantReading | None: Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. """ - forces, ts = self._sensor_client.get_auto_latest() + forces, ts = self._tactile_client.get_auto_latest() if forces is None: return None return ResultantReading(forces=forces, timestamp=ts) @@ -1271,30 +1271,47 @@ def get_tactile_taxels(self) -> TaxelReading | None: Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. """ - taxels, ts = self._sensor_client.get_auto_latest_taxels() + 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._sensor_client.start_auto_stream( + self._tactile_client.start_auto_stream( resultant=resultant, taxels=taxels, min_sensors=min_sensors, ) def stop_tactile_stream(self) -> None: - self._sensor_client.stop_auto_stream() + 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._sensor_client.capture_taxel_offsets(num_samples=num_samples) + return self._tactile_client.capture_taxel_offsets(num_samples=num_samples) def clear_tactile_zero(self) -> None: - self._sensor_client.clear_taxel_offsets() + self._tactile_client.clear_taxel_offsets() - def get_sensor_configuration(self): - return self._sensor_client.get_sensor_configuration() + def get_tactile_configuration(self): + return self._tactile_client.get_tactile_configuration() def get_tactile_stats(self): """Return ``AutoStreamStats`` for the running auto-stream. @@ -1302,7 +1319,7 @@ def get_tactile_stats(self): Useful for monitoring stream health (``frames_ok``, ``frames_bad_checksum``, ``parse_errors``, ``resyncs``, ``last_error_code``). """ - return self._sensor_client.get_auto_stats() + return self._tactile_client.get_auto_stats() class MockOrcaHand(OrcaHand): @@ -1336,10 +1353,8 @@ def _create_motor_client(self) -> MotorClient: self.config.motor_ids, self.config.port, self.config.baudrate ) - def _create_sensor_client(self): - from .hardware.mock_sensor_client import MockSensorClient - - return MockSensorClient( + 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/scripts/test_sensors.py b/scripts/test_sensors.py index 01316588..e563dda0 100644 --- a/scripts/test_sensors.py +++ b/scripts/test_sensors.py @@ -238,7 +238,7 @@ def detect_wiring_mismatch(target_finger, peaks, wiring, threshold=PRESS_THRESHO def phase_1_enumerate(hand): banner(1, "Connect & enumerate") - cfg = hand.get_sensor_configuration() + cfg = hand.get_tactile_configuration() print(f" {cfg}") print(" Per-finger status (canonical order):") for f in FINGERS: @@ -323,7 +323,7 @@ def phase_3_taxels_press(hand): for f in FINGERS: if f not in reading: return False, f"{f} missing from taxel frame" - n_expected = hand.get_sensor_configuration().num_taxels[f] + 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}") @@ -336,7 +336,7 @@ def phase_3_taxels_press(hand): warnings = [] for f in FINGERS: pause(f"press {f.upper()} (move your finger around to light up different taxels)") - n = hand.get_sensor_configuration().num_taxels[f] + 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) diff --git a/tests/test_tactile_sensor.py b/tests/test_tactile_sensor.py index 69ce8568..b1f525eb 100644 --- a/tests/test_tactile_sensor.py +++ b/tests/test_tactile_sensor.py @@ -1,4 +1,4 @@ -"""Tests for MockSensorClient integration and SensorConfiguration contracts. +"""Tests for MockTactileClient integration and TactileSensorConfiguration contracts. Validates the mock's lifecycle (connect → stream → read → stop), offset logic, and configuration ordering. Pure protocol codec tests live in test_protocol.py. @@ -6,8 +6,8 @@ import pytest -from orca_core.hardware.sensor_client import SensorConfiguration -from orca_core.hardware.mock_sensor_client import MockSensorClient +from orca_core.hardware.tactile_client import TactileSensorConfiguration +from orca_core.hardware.mock_tactile_client import MockTactileClient from orca_core.hardware.sensing.constants import DEFAULT_TAXEL_COUNTS from orca_core.hardware.sensing.protocol import compute_distal_module_index @@ -16,8 +16,8 @@ @pytest.fixture def mock(): - """Connected MockSensorClient with all fingers, cleaned up on teardown.""" - client = MockSensorClient(connected_sensors=ALL_FINGERS) + """Connected MockTactileClient with all fingers, cleaned up on teardown.""" + client = MockTactileClient(connected_sensors=ALL_FINGERS) client.connect() yield client client.disconnect() @@ -29,7 +29,7 @@ def mock_factory(): created = [] def _make(connected_sensors, **kwargs): - client = MockSensorClient(connected_sensors=connected_sensors, **kwargs) + client = MockTactileClient(connected_sensors=connected_sensors, **kwargs) client.connect() created.append(client) return client @@ -43,7 +43,7 @@ def _make_config( connected_fingers: list[str], taxel_counts: dict[str, int] | None = None, finger_to_sensor_id: dict[str, int] | None = None, -) -> SensorConfiguration: +) -> TactileSensorConfiguration: if taxel_counts is None: taxel_counts = DEFAULT_TAXEL_COUNTS if finger_to_sensor_id is None: @@ -53,7 +53,7 @@ def _make_config( 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 SensorConfiguration( + return TactileSensorConfiguration( connected=connected, num_taxels=num_taxels, module_indices=module_indices, @@ -135,7 +135,7 @@ def test_combined_mode_returns_both_streams(mock): def test_custom_provider_is_used(kind): if kind == "resultant": marker = [4.2, -3.0, 20.0] - mock = MockSensorClient( + mock = MockTactileClient( connected_sensors=["thumb"], resultant_provider=lambda: {"thumb": marker}, ) @@ -148,7 +148,7 @@ def test_custom_provider_is_used(kind): assert result["thumb"] == marker else: marker = [[9.9, -8.8, 7.7]] - mock = MockSensorClient( + mock = MockTactileClient( connected_sensors=["thumb"], taxel_counts={"thumb": 1}, taxel_provider=lambda: {"thumb": marker}, @@ -231,7 +231,7 @@ def test_taxel_offsets_applied_in_stream(mock_factory): # --------------------------------------------------------------------------- def test_read_before_connect_raises(): - mock = MockSensorClient(connected_sensors=ALL_FINGERS) + mock = MockTactileClient(connected_sensors=ALL_FINGERS) with pytest.raises(OSError, match="connect"): mock.read_resultant_force() @@ -243,7 +243,7 @@ def test_get_auto_latest_before_stream_returns_none(mock): # --------------------------------------------------------------------------- -# SensorConfiguration ordering +# TactileSensorConfiguration ordering # --------------------------------------------------------------------------- def test_slot_order_default(): From b68fb05946ceca71b57cffec4ab1a6700367577b Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Wed, 6 May 2026 14:59:45 +0200 Subject: [PATCH 17/20] -Revert .gitignore chagnes unrelated to tactile sensing --- .gitignore | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index 02554b94..7d946550 100644 --- a/.gitignore +++ b/.gitignore @@ -93,7 +93,6 @@ celerybeat-schedule # Environments .env .venv -uv.lock env/ venv/ ENV/ @@ -124,15 +123,9 @@ urdf/ dev_scripts/ -calibration.yaml +orca_core/models/* # macOS .DS_Store .pytest_cache/ - -# not storing the lockfile (as of now) -uv.lock - -# Vendor reference documents -docs/references/ From 0917b102eb9d3e13577e02ed16f1093d82eca087 Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Wed, 6 May 2026 15:23:55 +0200 Subject: [PATCH 18/20] Tighten tactile-sensing comments, drop unused mock kwarg, add pyserial --- orca_core/hardware/mock_tactile_client.py | 37 +- orca_core/hardware/sensing/constants.py | 9 +- orca_core/hardware/sensing/protocol.py | 198 ++-------- orca_core/hardware/tactile_client.py | 441 ++++------------------ orca_core/hardware_hand.py | 24 +- scripts/test_sensors.py | 7 +- 6 files changed, 126 insertions(+), 590 deletions(-) diff --git a/orca_core/hardware/mock_tactile_client.py b/orca_core/hardware/mock_tactile_client.py index 4bc9e1a9..1e5dedd5 100644 --- a/orca_core/hardware/mock_tactile_client.py +++ b/orca_core/hardware/mock_tactile_client.py @@ -11,7 +11,6 @@ from collections.abc import Callable import threading -import time import logging from orca_core.hardware.tactile_client import TactileClient @@ -23,7 +22,6 @@ AUTO_DATA_TAXELS, ) from orca_core.hardware.sensing.protocol import ( - ForceVector, ResultantForces, TaxelForces, decode_combined_auto, @@ -65,7 +63,6 @@ def __init__( finger_to_sensor_id: dict[str, int] | None = None, resultant_provider: ResultantProvider | None = None, taxel_provider: TaxelProvider | None = None, - auto_rate_hz: float | None = None, ): super().__init__(port=port, baudrate=baudrate, finger_to_sensor_id=finger_to_sensor_id) @@ -92,12 +89,7 @@ def __init__( taxel_provider if taxel_provider is not None else self._default_taxel_provider ) - # None = no sleep between frames (ideal for tests). Pass e.g. 1000 - # to throttle to ~1kHz for demos or UI prototyping. - self._auto_rate_hz = auto_rate_hz - - # Set by _acquire_frame after the first frame is produced. Lets tests - # synchronize on stream start without polling/sleeping. + # Lets tests synchronize on stream start without polling. self._first_frame_event = threading.Event() # ========================================================================= @@ -105,24 +97,12 @@ def __init__( # ========================================================================= def set_mock_forces(self, forces: ResultantForces) -> None: - """Set the force values to return for each sensor. - - Replaces all previously set mock forces. - - Raises: - ValueError: If any finger name is not valid or force vector is wrong length - """ + """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: - """Set the taxel values to return for each sensor. - - Replaces all previously set mock taxels. - - Raises: - ValueError: If any finger name is not valid or any taxel vector is wrong length - """ + """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") @@ -190,7 +170,7 @@ def read_auto_data_type(self) -> dict: def _write_register(self, address: int, data: bytes, response_timeout_s: float = 0.5) -> None: pass -# ========================================================================= + # ========================================================================= # Force Reading # ========================================================================= @@ -217,11 +197,7 @@ def start_auto_stream(self, *args, **kwargs): 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. - - Lets tests synchronize on stream startup without polling or sleeps. - Raises TimeoutError if no frame arrives in `timeout` seconds. - """ + """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") @@ -261,9 +237,6 @@ def _acquire_frame( parsed_resultant = None parsed_taxels = None - if self._auto_rate_hz: - time.sleep(1.0 / self._auto_rate_hz) - return parsed_resultant, parsed_taxels # ========================================================================= diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py index 607fdd26..408ce42e 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -95,12 +95,9 @@ DISTAL_MODULE_OFFSET = 2 """Offset of the distal phalanx module within a slot's module group.""" -# Hardware slot bit positions in the connected-sensors register. -# Each slot has a fixed (byte_index, bit_position) in the 4-byte register block. -# These describe physical board layout — independent of finger_to_sensor_id mapping. -# The finger_to_sensor_id mapping is applied on top to translate slot → finger name. +# 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)] -# Hardware register addresses for each slot's distal-phalanx taxel count. -# Same as above: fixed board layout, finger mapping applied separately. +# 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 index 8379f898..26364d72 100644 --- a/orca_core/hardware/sensing/protocol.py +++ b/orca_core/hardware/sensing/protocol.py @@ -87,18 +87,10 @@ def calculate_checksum(frame: bytes) -> int: def validate_auto_frame_lrc(meta: bytes, payload: bytes, lrc: int) -> bool: - """Check LRC of an auto-stream frame. Returns True if valid. + """Return ``True`` if ``lrc`` matches the checksum over ``header + meta + payload``. - Reconstructs the full frame (header + meta + payload) internally so - the caller doesn't need to know the frame assembly recipe. - - Note: returns bool (not raises) because the auto-stream reader counts - bad-LRC frames as a recoverable statistic rather than aborting. - - Args: - meta: 3 bytes after the AA56 header (reserved + eff_len) - payload: The payload bytes (eff_len bytes) - lrc: The LRC byte to validate against + 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 @@ -115,11 +107,7 @@ def _validate_frame_lrc(frame: bytes, context: str) -> None: # ========================================================================= def read_response_body_size(count: int) -> int: - """Total bytes after the AA55 header in a read response. - - Args: - count: Number of data bytes requested (same value passed to build_read_request) - """ + """Bytes after the AA55 header in a read response with ``count`` data bytes.""" return RESPONSE_META_SIZE + count + 1 # meta + data + LRC @@ -169,15 +157,10 @@ def build_write_request(address: int, data: bytes) -> bytes: # ========================================================================= def parse_read_response(frame: bytes) -> bytes: - """Validate and extract data from a read response frame. - - Frame layout: header(2) + reserved(1) + func(1) + addr(2) + count(2) + data(count) + LRC(1) - - Returns: - The data bytes from the response + """Validate a read-response frame and return its data bytes. - Raises: - IOError: If frame is too short, func code is wrong, or LRC validation fails + 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( @@ -206,29 +189,17 @@ def parse_read_response(frame: bytes) -> bytes: def extract_write_response_data_length(meta: bytes) -> int: - """Extract payload length from write response meta bytes. - - Args: - meta: Exactly 6 bytes (reserved + func + addr + nbytes) - - Returns: - Number of payload bytes that follow the meta - - Raises: - ValueError: If meta is not exactly 6 bytes - """ + """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 (LRC and status check). + """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: If frame is too short, LRC validation fails, or status byte indicates failure + 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( @@ -258,16 +229,10 @@ def parse_write_response(frame: bytes) -> None: # ========================================================================= def extract_auto_frame_eff_len(meta: bytes) -> int: - """Extract effective length from auto-stream frame meta bytes. - - Args: - meta: 3 bytes after the AA56 header (reserved(1) + eff_len(2)) + """Read eff_len (includes error_code byte) from the 3-byte auto-frame meta. - Returns: - Effective payload length (includes error_code byte) - - Raises: - ValueError: If eff_len exceeds MAX_AUTO_FRAME_EFF_LEN (possible corruption) + 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: @@ -276,16 +241,10 @@ def extract_auto_frame_eff_len(meta: bytes) -> int: def unpack_auto_payload(payload: bytes) -> tuple[int, bytes]: - """Unpack auto-stream payload into error code and force data. - - The protocol packs a 1-byte sensor error code (0 = no error) followed - by the actual force data into a single payload. This separates them. + """Split an auto-stream payload into (error_code, force_data). - Returns: - (error_code, force_data) tuple - - Raises: - ValueError: If payload is empty + 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)") @@ -324,14 +283,7 @@ def compute_expected_payload_size( active_sensors: list[str], num_taxels: dict[str, int], ) -> int: - """Compute expected auto-stream payload size for the given streaming mode. - - Args: - mode_resultant: Whether resultant force data is enabled - mode_taxels: Whether taxel data is enabled - active_sensors: List of active finger names - num_taxels: {finger: taxel_count} for each finger - """ + """Compute expected auto-stream payload size for the given streaming mode.""" if mode_resultant and mode_taxels: return compute_combined_payload_size(active_sensors, num_taxels) elif mode_resultant: @@ -382,21 +334,10 @@ def decode_resultant_auto( data: bytes, active_sensors: list[str], ) -> ResultantForces: - """Decode auto-stream resultant forces (6 bytes/sensor, sequential). - - Args: - data: Raw byte data from auto-stream - active_sensors: Finger names sorted by hardware slot ID ascending. - Auto-stream data arrives in slot order, so this ordering is - required for correct finger-to-data mapping. Ordering is the - caller's responsibility — the codec does not validate it because - it has no access to the slot-ID mapping. + """Decode auto-stream resultant forces (6 bytes/sensor, in slot order). - Returns: - Resultant forces for each active sensor - - Raises: - ValueError: If data size doesn't match expected + ``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)") @@ -414,20 +355,7 @@ def decode_taxels_auto( ) -> TaxelForces: """Decode auto-stream taxel data (3 bytes/taxel, sequential by sensor). - Args: - data: Raw byte data from auto-stream - active_sensors: Finger names sorted by hardware slot ID ascending. - Auto-stream data arrives in slot order, so this ordering is - required for correct finger-to-data mapping. Ordering is the - caller's responsibility — the codec does not validate it because - it has no access to the slot-ID mapping. - num_taxels: {finger: taxel_count} for each finger - - Returns: - Per-taxel forces for each active sensor - - Raises: - ValueError: If data size doesn't match expected + ``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") @@ -448,25 +376,9 @@ def decode_combined_auto( active_sensors: list[str], num_taxels: dict[str, int], ) -> tuple[ResultantForces, TaxelForces]: - """Decode auto-stream combined format (resultant + taxels interleaved per sensor). - - For each sensor in slot order: resultant(6 bytes) then taxels(3 bytes each), - followed by the next sensor's resultant + taxels, and so on. + """Decode interleaved auto-stream: per sensor, resultant(6) + taxels(3 each). - Args: - data: Raw byte data from auto-stream - active_sensors: Finger names sorted by hardware slot ID ascending. - Auto-stream data arrives in slot order, so this ordering is - required for correct finger-to-data mapping. Ordering is the - caller's responsibility — the codec does not validate it because - it has no access to the slot-ID mapping. - num_taxels: {finger: taxel_count} for each finger - - Returns: - Tuple of (resultant forces, per-taxel forces) - - Raises: - ValueError: If data size doesn't match expected + ``active_sensors`` must already be in slot order (see ``decode_resultant_auto``). """ expected_size = compute_combined_payload_size( active_sensors, num_taxels, @@ -495,17 +407,11 @@ def decode_combined_auto( # ========================================================================= def compute_distal_module_index(sensor_id: int) -> int: - """Compute the register-block module index for a fingertip (distal phalanx) sensor. + """Module index of the distal phalanx for ``sensor_id`` (slot 0-4). - The resultant force register block contains MODULES_PER_SLOT modules per - sensor slot (proximal, middle, distal, nail). The distal phalanx is at - offset DISTAL_MODULE_OFFSET within each slot's group. - - Args: - sensor_id: Hardware slot ID (0-4) - - Returns: - Module index into the 28-module resultant force register block + 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 @@ -515,24 +421,11 @@ def decode_resultant_register( active_sensors: list[str], module_indices: dict[str, int], ) -> ResultantForces: - """Decode the full 168-byte resultant force register block (0x0500-0x05A7). - - Used in request-response mode where the full register block is returned. - The block contains 28 modules (MODULES_PER_SLOT per sensor slot: proximal, - middle, distal, nail; plus 8 palm modules), each with 6 bytes (fx, fy, fz). - Use compute_distal_module_index() to get the correct module index for - fingertip sensors. + """Decode the 168-byte resultant register block (0x0500-0x05A7) at given indices. - Args: - data: Raw 168-byte register block - active_sensors: Finger names to extract - module_indices: {finger: module_idx} — zero-based index into the - 28-module block. Byte offset = module_idx * BYTES_PER_RESULTANT. - For fingertip sensors, use compute_distal_module_index(slot_id) - to get the correct index. - - Returns: - Resultant forces for each active sensor + 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. Raises: ValueError: If data is too short @@ -554,18 +447,7 @@ def decode_connected_sensors( data: bytes, sensor_id_to_finger: dict[int, str], ) -> dict[str, bool]: - """Decode connected-sensors register (4 bytes) into {finger: bool}. - - Args: - data: 4-byte register block - sensor_id_to_finger: {slot_id: finger_name} mapping for all slots - - Returns: - {finger: is_connected} for each slot - - Raises: - ValueError: If data is too short or sensor_id_to_finger doesn't cover all slots - """ + """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)}") @@ -582,18 +464,7 @@ def decode_num_taxels( data: bytes, sensor_id_to_finger: dict[int, str], ) -> dict[str, int]: - """Decode taxel-count register block into {finger: count}. - - Args: - data: 56-byte register block (28 x uint16 little-endian) - sensor_id_to_finger: {slot_id: finger_name} mapping for all slots - - Returns: - {finger: taxel_count} for each slot - - Raises: - ValueError: If data is too short or sensor_id_to_finger doesn't cover all slots - """ + """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( @@ -605,7 +476,6 @@ def decode_num_taxels( f"sensor_id_to_finger must have {num_slots} entries, " f"got {len(sensor_id_to_finger)}" ) - # Length validated above as ADDR_NUM_TAXELS_LENGTH (56), guaranteeing even byte count taxel_counts = [ int.from_bytes(data[i:i+2], byteorder="little") for i in range(0, len(data), 2) diff --git a/orca_core/hardware/tactile_client.py b/orca_core/hardware/tactile_client.py index 989fc0d7..db7d068e 100644 --- a/orca_core/hardware/tactile_client.py +++ b/orca_core/hardware/tactile_client.py @@ -161,39 +161,30 @@ def __init__(self, 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()} - # Sensor configuration (dynamic, adapts to connected sensors) self._tactile_config: TactileSensorConfiguration | None = None self._auto_thread: threading.Thread | None = None - self._auto_running = threading.Event() # Thread-safe flag for auto stream + self._auto_running = threading.Event() self._auto_lock = threading.Lock() - self._auto_latest = None # parsed resultant forces dict - self._auto_latest_taxels = None # parsed taxels dict + self._auto_latest = None + self._auto_latest_taxels = None self._auto_latest_ts = None self._auto_stats = AutoStreamStats() - self._auto_mode_resultant = True # Whether to parse resultant forces - self._auto_mode_taxels = False # Whether to parse taxels + self._auto_mode_resultant = True + self._auto_mode_taxels = False self._last_frame_debug_print: float = 0.0 - # Per-taxel zeroing offsets - self._taxel_offsets: dict | None = None # {finger: [[fx, fy, fz], ...], ...} - self._resultant_offsets: dict | None = None # {finger: [fx, fy, fz], ...} + # {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: - """Check if client is connected.""" return self._connected def connect(self): - """Connect to the sensor device and get initial configuration. - - This method establishes serial communication and reads the initial sensor - configuration (which sensors are connected, taxel counts, etc.). - - Raises: - ConnectionError: If serial connection fails - IOError: If unable to read sensor configuration - """ + """Open the serial link and capture the initial sensor configuration.""" if self.is_connected: return @@ -209,13 +200,12 @@ def connect(self): self._connected = True logger.info(f"Connected to sensor at {self.port}") - # Get initial sensor configuration + # 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}") - # Don't fail connection, config will be retrieved when starting auto-stream except (serial.SerialException, OSError) as e: raise ConnectionError(f"Failed to connect to sensor at {self.port}: {e}") from e @@ -230,41 +220,22 @@ def disconnect(self): self._connected = False def _read_register(self, address: int, count: int = 1, response_timeout_s: float = 0.5) -> bytes: - """Read one or more registers - - Protocol flow: - 1. Send request frame: 55 AA | reserved | 0x03 | addr(2) | count(2) | LRC - 2. Wait for response frame: AA 55 | reserved | 0x03 | addr(2) | count(2) | data(count) | LRC - 3. Handle auto frames (AA 56) that may arrive while waiting for response - - This method is robust against auto-stream mode: while waiting for the AA55 - response, any AA56 auto frames that arrive are skipped automatically. - - Args: - address: Register address to read from - count: Number of bytes to read - response_timeout_s: Maximum time to wait for response (default: 0.5s) + """Send a read request and return the data bytes from the response. - Returns: - Raw bytes read from registers - - Raises: - OSError: If not connected - TimeoutError: If no response received within timeout - IOError: If response checksum fails + 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) - # Clear stale data if not streaming (prevents reading old responses) + # 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) - # Wait for AA55 response header, skipping any AA56 auto frames deadline = time.time() + response_timeout_s while True: remaining = deadline - time.time() @@ -272,66 +243,41 @@ def _read_register(self, address: int, count: int = 1, response_timeout_s: float raise TimeoutError("Timed out waiting for read response (AA55).") hdr = self._read_header_resync(timeout_s=remaining) - if hdr == PROTOCOL_HEADER_AUTO: # AA56 auto frame + if hdr == PROTOCOL_HEADER_AUTO: self._skip_auto_frame() continue - break # Found AA55 response + break body = self._read_exact(read_response_body_size(count)) return parse_read_response(hdr + body) def _skip_auto_frame(self) -> None: - """Skip one complete auto-stream frame after having consumed the AA56 header. - - This is called when waiting for a request-response (AA55) frame but an - auto-stream (AA56) frame arrives first. We skip it to continue waiting - for the AA55 response. - - Raises: - IOError: If serial read fails - ValueError: If eff_len is unreasonably large (>8KB) - """ + """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: - """Read bytes until we find either AA55 (response) or AA56 (auto) header. + """Slide a 2-byte window until AA55 (response) or AA56 (auto) is found. - Uses a sliding 2-byte window to locate frame headers even if the byte - stream starts mid-frame or is misaligned. This is critical for robustness - when auto-stream frames (AA56) can arrive at any time, even when waiting - for request-response frames (AA55). - - Args: - timeout_s: Maximum time to search for a header before giving up - - Returns: - The 2-byte header (either AA55 or AA56) - - Raises: - TimeoutError: If no valid header found within timeout_s + 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 - - # Sliding 2-byte window: [b1, b] b1 = b"" while time.time() < deadline: b = self._serial_connection.read(1) if not b: - continue # Serial timeout tick, keep trying until deadline + continue - # Build up 2-byte window if not b1: b1 = b continue hdr = b1 + b - # Check if we found a valid header if hdr == PROTOCOL_HEADER_RESPONSE or hdr == PROTOCOL_HEADER_AUTO: return hdr - # Slide window: b becomes new b1 b1 = b raise TimeoutError("Timed out waiting for AA55/AA56 header.") @@ -345,39 +291,21 @@ def _write_register( data: bytes, response_timeout_s: float = 0.5, ) -> None: - """Write bytes to registers - - Protocol flow: - 1. Send request: 55 AA | reserved | 0x10 | addr(2) | len(2) | data | LRC - 2. Wait for response: AA 55 | reserved | 0x10 | addr(2) | len(2) | status | LRC - 3. Handle auto frames (AA 56) that may arrive while waiting for response - 4. Check status byte (0x00 = success) - - This method is robust against auto-stream mode: while waiting for the AA55 - response, any AA56 auto frames that arrive are skipped automatically. - - Args: - address: Register address to write to - data: Bytes to write (max 10 bytes according to manual) - response_timeout_s: Maximum time to wait for response (default: 0.5s) - - Raises: - OSError: If not connected - TimeoutError: If no response received within timeout - IOError: If response checksum fails or status byte indicates failure + """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) - # Clear stale data if not streaming (prevents reading old responses) if not self._is_streaming(): self._serial_connection.reset_input_buffer() self._serial_connection.write(request) - # Wait for AA55 response header, skipping any AA56 auto frames deadline = time.time() + response_timeout_s while True: remaining = deadline - time.time() @@ -385,15 +313,11 @@ def _write_register( raise TimeoutError("Timed out waiting for write response (AA55).") hdr = self._read_header_resync(timeout_s=remaining) - - if hdr == PROTOCOL_HEADER_AUTO: # AA56 auto frame + if hdr == PROTOCOL_HEADER_AUTO: self._skip_auto_frame() continue - - # Found AA55 response break - # Read and parse response: header(2) + meta(6) + payload(nbytes) + LRC(1) meta = self._read_exact(RESPONSE_META_SIZE) data_len = extract_write_response_data_length(meta) rest = self._read_exact(data_len + 1) # payload + LRC @@ -402,14 +326,7 @@ def _write_register( def read_connected_sensors(self) -> dict[str, bool]: - """Read the connected sensors. - - Returns: - Dictionary of sensor names and their status - - Raises: - OSError: If not connected to sensor - """ + """Return ``{finger: is_connected}`` from the connected-sensors register.""" if not self.is_connected: raise OSError("Must call connect() first.") @@ -417,14 +334,7 @@ def read_connected_sensors(self) -> dict[str, bool]: return decode_connected_sensors(data, self._sensor_id_to_finger) def read_num_taxels(self) -> dict[str, int]: - """Read the number of taxels for each fingertip sensor. - - Returns: - Dictionary mapping finger names to taxel counts - - Raises: - OSError: If not connected to sensor - """ + """Return ``{finger: taxel_count}`` from the taxel-count register block.""" if not self.is_connected: raise OSError("Must call connect() first.") @@ -432,39 +342,20 @@ def read_num_taxels(self) -> dict[str, int]: return decode_num_taxels(data, self._sensor_id_to_finger) def read_auto_data_type(self) -> dict: - """Read the auto-stream data-type register. - - Returns the configured payload format for auto-stream frames (which of - resultant force / individual taxels are included). - - Returns: - Dict with the raw register byte and decoded resultant/taxels flags. - - Raises: - OSError: If not connected to sensor. - """ + """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 (no offset application). - - MockTactileClient overrides this to return simulated - data. The public read_resultant_force() method calls this, then applies - zeroing offsets. - - Returns: - Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons - """ - # Ensure we have current configuration + """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 static parsing using default module indices + # 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) @@ -475,17 +366,7 @@ def _read_raw_resultant(self) -> dict[str, list[float]]: ) def read_resultant_force(self) -> dict[str, list[float]]: - """Read resultant force from all connected fingertip sensors. - - Calls _read_raw_resultant() for data, then applies zeroing offsets. - - Returns: - Dictionary mapping finger names to [fx, fy, fz] force vectors in Newtons - Only includes sensors that are currently connected - - Raises: - OSError: If not connected to sensor - """ + """Read resultant force from all connected fingertip sensors, applying offsets.""" if not self.is_connected: raise OSError("Must call connect() first.") @@ -495,24 +376,11 @@ def read_resultant_force(self) -> dict[str, list[float]]: return result def get_tactile_configuration(self) -> TactileSensorConfiguration | None: - """Get the current sensor configuration snapshot. - - Returns: - TactileSensorConfiguration object with current sensor state, or None if not yet configured - """ + """Return the cached sensor configuration, or ``None`` if never read.""" return self._tactile_config def _get_configuration(self) -> TactileSensorConfiguration: - """Snapshot the current sensor configuration at stream start. - - Reads connected sensors and their properties from the hardware. - - Returns: - TactileSensorConfiguration with current hardware state - - Raises: - IOError: If unable to read configuration from sensor - """ + """Read the current connected-sensors and taxel-count registers from hardware.""" try: connected = self.read_connected_sensors() num_taxels = self.read_num_taxels() @@ -538,15 +406,7 @@ def _get_configuration(self) -> TactileSensorConfiguration: 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 to include in auto stream. - - Args: - resultant: Include resultant force data - taxels: Include individual taxel force data - - Raises: - OSError: If not connected to sensor - """ + """Configure which data types are included in auto-stream frames.""" if not self.is_connected: raise OSError("Must call connect() first.") @@ -554,22 +414,14 @@ def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> No def enable_auto_data_transmission(self) -> None: - """Enable automatic data transmission mode. - - Raises: - OSError: If not connected to sensor - """ + """Enable automatic data transmission mode.""" 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: - """Disable automatic data transmission mode. - - Raises: - OSError: If not connected to sensor - """ + """Disable automatic data transmission mode.""" if not self.is_connected: raise OSError("Must call connect() first.") @@ -577,63 +429,30 @@ def disable_auto_data_transmission(self) -> None: def get_auto_latest(self): - """Thread-safe snapshot of the most recent resultant-force frame. - - Updated by the background reader thread while auto-stream is active. - - Returns: - (forces, timestamp): `forces` is a {finger: [fx, fy, fz]} dict in - Newtons, or None if no frame has arrived or resultant mode is off. - `timestamp` is the wall-clock time of receipt, or None. - """ + """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): - """Thread-safe snapshot of the most recent per-taxel frame. - - Only populated when auto-stream was started with `taxels=True`. - - Returns: - (taxels, timestamp): `taxels` is a {finger: [[fx, fy, fz], ...]} - dict (one [fx, fy, fz] per taxel, in Newtons), or None if no frame - has arrived or taxel mode is off. `timestamp` is the wall-clock - time of receipt, or None. - """ + """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): - """Thread-safe snapshot of both resultant and taxel data in one call. - - Useful in combined mode (`resultant=True, taxels=True`) to get a - consistent view without two separate locked reads. + """Return ``(forces, taxels, timestamp)`` from a single locked read. - Returns: - (forces, taxels, timestamp). Any field may be None depending on - the active stream mode and whether a frame has arrived yet. + 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): - """Thread-safe snapshot of auto-stream diagnostics. - - Returns a copy so callers can compare values across calls without - racing the reader thread. - - See `AutoStreamStats` for the meaning of each field. Useful for health - monitoring (frame rate, checksum errors, parse errors). - """ + """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: - """Set per-taxel zeroing offsets and compute resultant offsets. - Args: - offsets: {finger: [[fx, fy, fz], ...], ...} per-taxel offsets - """ + 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(): @@ -648,32 +467,25 @@ def clear_taxel_offsets(self) -> None: self._resultant_offsets = None def capture_taxel_offsets(self, num_samples: int = 100) -> dict: - """Capture live baseline offsets by averaging current sensor readings. - - Requires active auto-stream with taxels enabled. Temporarily clears - any existing offsets so raw sensor data is captured. - - Args: - num_samples: Number of unique frames to average + """Average ``num_samples`` taxel frames and apply the result as zeroing offsets. - Returns: - Per-taxel offsets dict: {finger: [[fx, fy, fz], ...], ...} + 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") - # Temporarily clear offsets to capture raw data + # 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 - # Wait for at least one raw frame to flush old offset-applied data + # Let the reader thread emit at least one new frame past the offset clear. time.sleep(0.01) succeeded = False try: - # Collect unique frames by checking timestamps frames = [] last_ts = None while len(frames) < num_samples: @@ -683,7 +495,6 @@ def capture_taxel_offsets(self, num_samples: int = 100) -> dict: last_ts = ts time.sleep(0.002) - # Average per-taxel [fx, fy, fz] across all frames fingers = list(frames[0].keys()) offsets = {} for finger in fingers: @@ -705,7 +516,6 @@ def capture_taxel_offsets(self, num_samples: int = 100) -> dict: return offsets finally: if not succeeded: - # Restore previous offsets on failure self._taxel_offsets = prev_taxel self._resultant_offsets = prev_resultant @@ -734,75 +544,42 @@ def _apply_resultant_offsets(self, forces: dict) -> None: fvec[2] = round(max(0, fvec[2] - off[2]), 1) def _on_frame_stored(self) -> None: - """Hook called after a frame is stored in _auto_latest. Subclasses - may override to signal frame availability (e.g. for tests).""" + """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. - - Called by _auto_reader_loop implementations after parsing raw data. - """ + """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 from serial connection, blocking until complete. - - This is a fundamental building block for the protocol implementation. - Unlike serial.read(n) which may return fewer bytes, this guarantees - exactly n bytes are read or an error is raised. - - The serial connection has a 1.0s timeout (set in connect()). If no data - arrives within that window, this raises IOError. This prevents infinite - blocking on sensor disconnect or communication failure. - - Args: - n: Number of bytes to read - - Returns: - Exactly n bytes read from serial connection - - Raises: - IOError: If serial read times out (1.0s) or connection is closed - """ + """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: - # Serial timeout or disconnect 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: - """Scan byte stream until we see 0xAA 0x56 auto-stream header. - - Uses a sliding 2-byte window to find the header even if the stream - starts mid-frame or becomes misaligned. Exits cleanly when auto stream - is stopped via _auto_running.clear(). - - Performance note: Checking is_set() adds <0.01% overhead since serial I/O - (milliseconds) dominates over the flag check (microseconds). + """Slide a 2-byte window over the stream until 0xAA 0x56 is found. - Raises: - IOError: If auto stream is stopped while resyncing + Exits and raises IOError when ``_auto_running`` is cleared, so a + stop request unblocks this loop instead of hanging on the next read. """ - # Sliding 2-byte window to find AA 56 header b1 = self._read_exact(1) while self._auto_running.is_set(): b2 = self._read_exact(1) if b1 + b2 == PROTOCOL_HEADER_AUTO: - return # Found the header - # Slide window: b2 becomes new b1 + return b1 = b2 - # If we exit the loop, auto stream was stopped raise IOError("Auto stream stopped during resync") def _get_expected_payload_size(self, config: TactileSensorConfiguration) -> int: @@ -819,21 +596,10 @@ def _acquire_frame( parse_resultant: bool, parse_taxels: bool, ) -> tuple[dict | None, dict | None]: - """Acquire and return the next parsed (resultant, taxels) frame. - - Reads one auto-stream frame from serial, validates LRC, and parses - the data. - - Subclasses (e.g. MockTactileClient) override this to provide data from - other sources while inheriting the loop's stats, offset, and lifecycle logic. + """Read one frame from serial, validate LRC, decode according to mode. - Returns: - Tuple of (parsed_resultant, parsed_taxels). Either may be None - if not requested. - - Raises: - FrameError: Recoverable frame-level error (bad LRC, parse failure) - IOError: Serial communication failure or auto stream stopped + Overridden by ``MockTactileClient`` to swap the data source while + keeping the surrounding stats, offset, and lifecycle logic. """ self._resync_to_auto_header() @@ -885,11 +651,7 @@ def _acquire_frame( return parsed_resultant, parsed_taxels def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): - """Background thread that continuously reads and parses auto-stream frames. - - Calls _acquire_frame() to get parsed data, then applies offsets and updates - stats. Subclasses override _acquire_frame() to change the data source while - inheriting the shared loop logic. + """Background thread: acquire → offset → store → repeat. Error handling: - FrameError: recoverable (bad LRC / parse error). Count and continue. @@ -938,42 +700,16 @@ def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): logger.info("Auto reader loop exited") def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_sensors: int = 1): - """Start continuous auto-stream mode for real-time force data. - - In auto-stream mode, the sensor continuously broadcasts force data at ~1kHz - without requiring request-response polling. This provides much lower latency - and higher throughput than repeatedly calling read_resultant_force(). - - The data is read by a background thread and made available via: - - get_auto_latest(): for resultant force data - - get_auto_latest_taxels(): for taxel data - - get_auto_latest_all(): for both - - Sensor presence is determined at stream start. Paxini PX-6AX GEN3 - firmware enumerates sensors at power-on and does not report mid-stream - disconnects — a physically disconnected sensor's slot continues to - emit its last bytes until power-cycle. Restart the host process and - power-cycle the board to pick up wiring changes. - - Setup sequence: - 1. Stop any existing stream - 2. Get sensor configuration (which sensors are connected) - 3. Configure data type (0x0016: resultant and/or taxels) - 4. Clear serial buffer to remove stale data - 5. Enable auto transmission (0x0017 = 1) - 6. Start background reader thread - - Args: - resultant: Include resultant force data (fx, fy, fz per sensor) - taxels: Include individual taxel force data - min_sensors: Minimum number of sensors required at stream start - (default: 1). If fewer sensors are enumerated, - raises NoSensorsAvailableError. - - Raises: - OSError: If not connected to sensor - NoSensorsAvailableError: If fewer than min_sensors are available - ValueError: If neither resultant nor taxels is enabled + """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.") @@ -981,27 +717,22 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se if not resultant and not taxels: raise ValueError("At least one of resultant or taxels must be enabled") - # Stop any existing stream to ensure clean state self.stop_auto_stream() - # Store mode settings for the reader loop self._auto_mode_resultant = resultant self._auto_mode_taxels = taxels - # Get initial sensor configuration try: self._tactile_config = self._get_configuration() except IOError as e: raise OSError(f"Failed to get sensor configuration: {e}") from e - # Check minimum sensor requirement 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}" ) - # Log expected payload size for debugging expected_size = self._get_expected_payload_size(self._tactile_config) mode_str = [] if resultant: @@ -1013,60 +744,46 @@ def start_auto_stream(self, resultant: bool = True, taxels: bool = False, min_se f"mode={'+'.join(mode_str)}, expected_payload={expected_size} bytes" ) - # Try to disable auto mode first (in case it was left enabled) + # 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 # If this fails, robust _write_register will handle AA56 frames + pass - # Configure which data types to include in auto frames self.set_auto_data_type(resultant=resultant, taxels=taxels) - # Clear any stale data from serial buffer before starting if self._serial_connection is not None: self._serial_connection.reset_input_buffer() - # Enable auto transmission mode (sensor starts broadcasting) self.enable_auto_data_transmission() - # Start background reader thread - self._auto_running.set() # Signal thread to run + self._auto_running.set() self._auto_thread = threading.Thread( target=self._auto_reader_loop, args=(resultant, taxels), - daemon=True # Thread exits when main program exits + daemon=True, ) self._auto_thread.start() def stop_auto_stream(self): - """Stop auto-stream mode and clean up background thread. - - Sequence: - 1. Signal thread to stop (_auto_running.clear()) - 2. Wait up to 1.0s for thread to exit gracefully - 3. Disable auto transmission on sensor (0x0017 = 0) - 4. Reset cached data + """Stop auto-stream mode and clean up the background thread. - This method is safe to call multiple times and handles cases where - the sensor is disconnected or the thread is already stopped. + Safe to call multiple times and when the sensor is already disconnected. """ - # Signal background thread to stop self._auto_running.clear() - # Wait for thread to exit (up to 1 second) if self._auto_thread is not None: self._auto_thread.join(timeout=1.0) self._auto_thread = None - # Disable auto mode on sensor (if still connected) if self.is_connected: try: self.disable_auto_data_transmission() except IOError: - pass # Ignore errors (e.g., if sensor disconnected) + pass - # Reset cached data with self._auto_lock: self._auto_latest = None self._auto_latest_taxels = None diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index 3a1be452..ff2866a0 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1249,28 +1249,14 @@ def disconnect(self) -> None: super().disconnect() def get_tactile_forces(self) -> ResultantReading | None: - """Return latest resultant force per finger, or ``None`` if unavailable. - - The returned object supports dict-style access by finger name:: - - reading["thumb"] # -> [fx, fy, fz] - - Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. - """ + """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 per-taxel forces, or ``None`` if unavailable. - - The returned object supports dict-style access by finger name:: - - reading["thumb"] # -> [[fx, fy, fz], ...] per taxel - - Available keys: ``"thumb"``, ``"index"``, ``"middle"``, ``"ring"``, ``"pinky"``. - """ + """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 @@ -1314,11 +1300,7 @@ def get_tactile_configuration(self): return self._tactile_client.get_tactile_configuration() def get_tactile_stats(self): - """Return ``AutoStreamStats`` for the running auto-stream. - - Useful for monitoring stream health (``frames_ok``, ``frames_bad_checksum``, - ``parse_errors``, ``resyncs``, ``last_error_code``). - """ + """Return ``AutoStreamStats`` for the running auto-stream.""" return self._tactile_client.get_auto_stats() diff --git a/scripts/test_sensors.py b/scripts/test_sensors.py index e563dda0..a480af55 100644 --- a/scripts/test_sensors.py +++ b/scripts/test_sensors.py @@ -19,11 +19,8 @@ from orca_core import OrcaHandTouch -# All thresholds, taxel layouts, and role mappings below live here rather -# than in orca_core.constants because they are only used by this health- -# check script — to decide pass/fail and to render a barebones ASCII view. -# They are not part of the OrcaHandTouch API and have no meaning at -# runtime for downstream code. +# 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"] From 2023fdb035eddcd0a812f02828b2b2f99a329ccf Mon Sep 17 00:00:00 2001 From: Fabrice Bourquin Date: Wed, 6 May 2026 16:57:51 +0200 Subject: [PATCH 19/20] Add FingerName Literal type and consolidate test_protocol.py --- orca_core/hardware/sensing/constants.py | 8 +- orca_core/hardware/sensing/types.py | 12 +- tests/test_protocol.py | 234 ++++++++++-------------- 3 files changed, 114 insertions(+), 140 deletions(-) diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py index 408ce42e..cd0d4d1e 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -1,10 +1,16 @@ """Constants for ORCA tactile sensing.""" +from typing import Literal + # --------------------------------------------------------------------------- # Client configuration defaults # --------------------------------------------------------------------------- -FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] +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 diff --git a/orca_core/hardware/sensing/types.py b/orca_core/hardware/sensing/types.py index a733f5e7..f709aec6 100644 --- a/orca_core/hardware/sensing/types.py +++ b/orca_core/hardware/sensing/types.py @@ -6,6 +6,8 @@ import numpy as np +from orca_core.hardware.sensing.constants import FingerName + @dataclass(frozen=True) class ResultantReading: @@ -17,10 +19,10 @@ class ResultantReading: forces: dict[str, list[float]] timestamp: float | None = None - def __getitem__(self, finger: str) -> list[float]: + def __getitem__(self, finger: FingerName) -> list[float]: return self.forces[finger] - def __contains__(self, finger: str) -> bool: + def __contains__(self, finger: FingerName) -> bool: return finger in self.forces @property @@ -43,17 +45,17 @@ class TaxelReading: taxels: dict[str, list[list[float]]] timestamp: float | None = None - def __getitem__(self, finger: str) -> list[list[float]]: + def __getitem__(self, finger: FingerName) -> list[list[float]]: return self.taxels[finger] - def __contains__(self, finger: str) -> bool: + 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: str) -> np.ndarray: + def as_array(self, finger: FingerName) -> np.ndarray: """Return an ``(n_taxels, 3)`` array for *finger*.""" return np.array(self.taxels[finger]) diff --git a/tests/test_protocol.py b/tests/test_protocol.py index ba110bc5..5b0196a2 100644 --- a/tests/test_protocol.py +++ b/tests/test_protocol.py @@ -30,6 +30,9 @@ 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, @@ -40,6 +43,7 @@ BYTES_PER_RESULTANT, BYTES_PER_TAXEL, MAX_AUTO_FRAME_EFF_LEN, + SLOT_DISTAL_TAXEL_REGISTER_OFFSETS, ) @@ -62,70 +66,59 @@ def _build_write_response(status: int) -> bytes: # Checksum # --------------------------------------------------------------------------- -def test_checksum_known_value(): - assert calculate_checksum(b"\x01\x02\x03") == 0xFA - - -def test_checksum_round_trip(): - frame = b"\xAA\x55\x00\x03\x10\x00\x04\x00" +@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 -def test_checksum_empty_frame(): - assert calculate_checksum(b"") == 0 - - -def test_checksum_single_byte(): - assert calculate_checksum(b"\x01") == 0xFF - - -def test_validate_auto_frame_lrc_valid(): - meta = b"\x00" + (3).to_bytes(2, "little") - payload = b"\x00\x01\x02" - frame_wo_lrc = b"\xAA\x56" + meta + payload - lrc = calculate_checksum(frame_wo_lrc) - assert validate_auto_frame_lrc(meta, payload, lrc) is True - - -def test_validate_auto_frame_lrc_invalid(): +@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" - assert validate_auto_frame_lrc(meta, payload, 0xFF) is False + 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 # --------------------------------------------------------------------------- -def test_read_response_body_size_known_value(): - # count=4 → meta(6) + data(4) + LRC(1) = 11 - assert read_response_body_size(4) == 11 - - -def test_read_response_body_size_single_byte(): - assert read_response_body_size(1) == 8 +@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_structure(): +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 - - -def test_build_read_request_lrc_valid(): - frame = build_read_request(address=0x0010, count=4) assert calculate_checksum(frame[:-1]) == frame[-1] -def test_build_write_request_structure(): +def test_build_write_request(): frame = build_write_request(address=0x0017, data=b"\x01") assert frame[:2] == PROTOCOL_HEADER_REQUEST assert frame[2] == 0x00 @@ -133,55 +126,40 @@ def test_build_write_request_structure(): assert int.from_bytes(frame[4:6], "little") == 0x0017 assert int.from_bytes(frame[6:8], "little") == 1 assert frame[8] == 0x01 - - -def test_build_write_request_lrc_valid(): - frame = build_write_request(address=0x0017, data=b"\x01") assert calculate_checksum(frame[:-1]) == frame[-1] -def test_build_read_request_zero_count_raises(): - with pytest.raises(ValueError, match="count must be > 0"): - build_read_request(address=0x0010, count=0) - - -def test_build_read_request_negative_count_raises(): - with pytest.raises(ValueError, match="count must be > 0"): - build_read_request(address=0x0010, count=-1) - - -def test_build_read_request_address_overflow_raises(): - with pytest.raises(ValueError, match="address"): - build_read_request(address=0x10000, count=1) - - -def test_build_read_request_negative_address_raises(): - with pytest.raises(ValueError, match="address"): - build_read_request(address=-1, count=1) - - -def test_build_write_request_empty_data_raises(): - with pytest.raises(ValueError, match="data must not be empty"): - build_write_request(address=0x0017, data=b"") +@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) -def test_build_write_request_address_overflow_raises(): - with pytest.raises(ValueError, match="address"): - build_write_request(address=0x10000, data=b"\x01") +@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 # --------------------------------------------------------------------------- -def test_parse_read_response_extracts_data(): - frame = _build_read_response(b"\xAB\xCD\xEF\x01") - assert parse_read_response(frame) == b"\xAB\xCD\xEF\x01" - - -def test_parse_read_response_single_byte(): - frame = _build_read_response(b"\x42") - assert parse_read_response(frame) == b"\x42" +@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(): @@ -265,15 +243,11 @@ def test_extract_write_response_data_length_wrong_size_raises(): # Auto-stream frame parsers # --------------------------------------------------------------------------- -def test_extract_auto_frame_eff_len_known_value(): - # reserved(1) + eff_len(2 LE) = 3 bytes - meta = b"\x00" + (42).to_bytes(2, "little") - assert extract_auto_frame_eff_len(meta) == 42 - - -def test_extract_auto_frame_eff_len_max_valid(): - meta = b"\x00" + MAX_AUTO_FRAME_EFF_LEN.to_bytes(2, "little") - assert extract_auto_frame_eff_len(meta) == MAX_AUTO_FRAME_EFF_LEN +@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(): @@ -282,22 +256,15 @@ def test_extract_auto_frame_eff_len_exceeds_max_raises(): extract_auto_frame_eff_len(meta) -def test_unpack_auto_payload_splits_error_and_data(): - err, data = unpack_auto_payload(b"\x00\x01\x02\x03") - assert err == 0 - assert data == b"\x01\x02\x03" - - -def test_unpack_auto_payload_nonzero_error_code(): - err, data = unpack_auto_payload(b"\x05\xAB") - assert err == 5 - assert data == b"\xAB" - - -def test_unpack_auto_payload_error_code_only(): - err, data = unpack_auto_payload(b"\x01") - assert err == 1 - assert data == b"" +@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 # --------------------------------------------------------------------------- @@ -457,26 +424,27 @@ def test_decode_resultant_register_too_short_raises(): # Register decoders — connected sensors # --------------------------------------------------------------------------- -def test_decode_connected_sensors_all_connected(): - # 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. - data = bytes([0x44, 0x44, 0x04, 0x00]) - result = decode_connected_sensors(data, ID_TO_FINGER) - assert all(result.values()) - - -def test_decode_connected_sensors_none_connected(): - result = decode_connected_sensors(bytes([0x00, 0x00, 0x00, 0x00]), ID_TO_FINGER) - assert not any(result.values()) - - -def test_decode_connected_sensors_partial(): - # Only slot 0 (bit 2 of byte 0) and slot 4 (bit 2 of byte 2) - data = bytes([0x04, 0x00, 0x04, 0x00]) - result = decode_connected_sensors(data, ID_TO_FINGER) - assert result["thumb"] is True - assert result["index"] is False - assert result["pinky"] is True +# 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(): @@ -494,14 +462,16 @@ def test_decode_connected_sensors_wrong_mapping_size_raises(): # --------------------------------------------------------------------------- def test_decode_num_taxels_known_values(): - # 28 uint16 values (56 bytes total). The distal register offsets - # [0x0034, 0x003C, 0x0044, 0x004C, 0x0054] at base 0x0030 correspond - # to indices [2, 6, 10, 14, 18] in the uint16 array. - data = bytearray(56) - for idx, count in zip([2, 6, 10, 14, 18], [51, 87, 87, 87, 51]): - struct.pack_into(" Date: Wed, 6 May 2026 17:14:01 +0200 Subject: [PATCH 20/20] Drop name-restating docstrings, add pyserial dep --- orca_core/hardware/sensing/constants.py | 1 - orca_core/hardware/sensing/protocol.py | 38 +++---------------------- orca_core/hardware/tactile_client.py | 27 ++---------------- orca_core/hardware_hand.py | 13 ++------- pyproject.toml | 1 + 5 files changed, 10 insertions(+), 70 deletions(-) diff --git a/orca_core/hardware/sensing/constants.py b/orca_core/hardware/sensing/constants.py index cd0d4d1e..9bbf8f73 100644 --- a/orca_core/hardware/sensing/constants.py +++ b/orca_core/hardware/sensing/constants.py @@ -23,7 +23,6 @@ "thumb": 51, "index": 87, "middle": 87, "ring": 87, "pinky": 51, } -# Finger-to-sensor-model mapping (replaces sensor_models.yaml) FINGER_MODELS = { "thumb": "touch-sensor-thumb", "index": "touch-sensor-finger", diff --git a/orca_core/hardware/sensing/protocol.py b/orca_core/hardware/sensing/protocol.py index 26364d72..69b339c6 100644 --- a/orca_core/hardware/sensing/protocol.py +++ b/orca_core/hardware/sensing/protocol.py @@ -2,15 +2,6 @@ Converts between raw bytes (as defined by the sensor hardware protocol) and Python objects. Pure functions only — no I/O, no state, no threading. - -Naming conventions: - build_* — assemble an outgoing request frame (bytes) - parse_* — validate an incoming frame and extract raw data (bytes) - decode_* — interpret raw bytes into domain objects (dicts of forces) - encode_* — convert domain values into register bytes - extract_* — pull a single field from frame metadata - unpack_* — split a payload into its component parts - compute_* — calculate sizes or indices from configuration """ from __future__ import annotations @@ -65,16 +56,6 @@ class AutoDataTypeInfo(TypedDict): taxels: bool -# ========================================================================= -# Protocol Constants -# ========================================================================= - -FORCE_DECIMAL_PLACES = 1 -"""Decimal places for rounding decoded force values. - -This is a codec-level choice, not a wire-format specification. The sensor -transmits integer LSB counts; this module converts to Newtons and rounds. -""" # ========================================================================= @@ -97,7 +78,6 @@ def validate_auto_frame_lrc(meta: bytes, payload: bytes, lrc: int) -> bool: def _validate_frame_lrc(frame: bytes, context: str) -> None: - """Validate LRC of a request-response frame. Raises on mismatch.""" if frame[-1] != calculate_checksum(frame[:-1]): raise IOError(f"{context} LRC mismatch") @@ -116,7 +96,6 @@ def read_response_body_size(count: int) -> int: # ========================================================================= def _validate_u16(value: int, name: str) -> None: - """Validate that a value fits in a uint16 field.""" if not 0 <= value <= 0xFFFF: raise ValueError(f"{name} must be 0x0000-0xFFFF, got {value}") @@ -256,21 +235,18 @@ def unpack_auto_payload(payload: bytes) -> tuple[int, bytes]: # ========================================================================= def compute_resultant_payload_size(num_sensors: int) -> int: - """Compute payload size for resultant-only auto-stream mode.""" return num_sensors * BYTES_PER_RESULTANT def compute_taxel_payload_size( active_sensors: list[str], num_taxels: dict[str, int], ) -> int: - """Compute payload size for taxel-only auto-stream mode.""" 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: - """Compute payload size for combined (resultant + taxels) auto-stream mode.""" return ( compute_resultant_payload_size(len(active_sensors)) + compute_taxel_payload_size(active_sensors, num_taxels) @@ -283,7 +259,7 @@ def compute_expected_payload_size( active_sensors: list[str], num_taxels: dict[str, int], ) -> int: - """Compute expected auto-stream payload size for the given streaming mode.""" + """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: @@ -298,7 +274,6 @@ def compute_expected_payload_size( # ========================================================================= def _validate_payload_size(data: bytes, expected: int, context: str) -> None: - """Validate that payload data matches expected size. Raises ValueError on mismatch.""" if len(data) != expected: preview = data[:16].hex() if data else "(empty)" raise ValueError( @@ -313,7 +288,7 @@ def _unpack_taxel(data: bytes, offset: int) -> ForceVector: 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, FORCE_DECIMAL_PLACES), round(fy, FORCE_DECIMAL_PLACES), round(fz, FORCE_DECIMAL_PLACES)] + return [round(fx, 1), round(fy, 1), round(fz, 1)] def _unpack_resultant(data: bytes, offset: int) -> ForceVector: @@ -327,7 +302,7 @@ def _unpack_resultant(data: bytes, offset: int) -> ForceVector: 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, FORCE_DECIMAL_PLACES), round(fy, FORCE_DECIMAL_PLACES), round(fz, FORCE_DECIMAL_PLACES)] + return [round(fx, 1), round(fy, 1), round(fz, 1)] def decode_resultant_auto( @@ -426,9 +401,6 @@ def decode_resultant_register( 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. - - Raises: - ValueError: If data is too short """ if len(data) < RESULTANT_BLOCK_SIZE: raise ValueError(f"Resultant force block too short: {len(data)} bytes") @@ -548,7 +520,6 @@ def encode_resultant_auto_for_mock( forces: ResultantForces, active_sensors: list[str], ) -> bytes: - """Encode resultant-only auto-stream payload for mock use.""" return b"".join(_pack_resultant_for_mock(forces[f]) for f in active_sensors) @@ -556,7 +527,6 @@ def encode_taxels_auto_for_mock( taxels: TaxelForces, active_sensors: list[str], ) -> bytes: - """Encode taxel-only auto-stream payload for mock use.""" return b"".join(_pack_taxel_for_mock(t) for f in active_sensors for t in taxels[f]) @@ -565,7 +535,7 @@ def encode_combined_auto_for_mock( taxels: TaxelForces, active_sensors: list[str], ) -> bytes: - """Encode interleaved (resultant + taxels) auto-stream payload for mock use.""" + """Build interleaved [resultant_i, taxels_i, resultant_{i+1}, ...] payload.""" out = bytearray() for f in active_sensors: out += _pack_resultant_for_mock(forces[f]) diff --git a/orca_core/hardware/tactile_client.py b/orca_core/hardware/tactile_client.py index db7d068e..4df8708e 100644 --- a/orca_core/hardware/tactile_client.py +++ b/orca_core/hardware/tactile_client.py @@ -60,7 +60,6 @@ class NoSensorsAvailableError(Exception): - """Raised when no sensors are available for communication.""" pass @@ -78,20 +77,12 @@ def __init__(self, message: str, bad_lrc: bool = False): @dataclass class AutoStreamStats: - """Diagnostic counters for the auto-stream reader loop. - - Attributes: - frames_ok: Frames received and decoded successfully. - frames_bad_checksum: Frames rejected due to checksum (LRC) mismatch. - parse_errors: Frames received intact but whose payload failed to decode. - resyncs: Times the reader had to resync after IO errors or bad framing. - last_error_code: Most recent sensor-reported error code (0 = no error). - """ + """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 + last_error_code: int = 0 # most recent sensor-reported error code (0 = no error) @dataclass @@ -120,11 +111,9 @@ def active_sensors(self) -> list[str]: @property def num_active_sensors(self) -> int: - """Number of currently connected sensors.""" return len(self.active_sensors) def __str__(self) -> str: - """Human-readable representation.""" active = ", ".join(self.active_sensors) if self.active_sensors else "none" return f"SensorConfig({self.num_active_sensors} active: {active})" @@ -211,10 +200,8 @@ def connect(self): raise ConnectionError(f"Failed to connect to sensor at {self.port}: {e}") from e def disconnect(self): - """Disconnect from the sensor device.""" if not self.is_connected: return - if self._serial_connection and self._serial_connection.is_open: self._serial_connection.close() self._connected = False @@ -414,17 +401,13 @@ def set_auto_data_type(self, resultant: bool = True, taxels: bool = False) -> No def enable_auto_data_transmission(self) -> None: - """Enable automatic data transmission mode.""" 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: - """Disable automatic data transmission mode.""" if not self.is_connected: raise OSError("Must call connect() first.") - self._write_register(ADDR_AUTO_ENABLE, REGISTER_DISABLE) @@ -652,12 +635,6 @@ def _acquire_frame( def _auto_reader_loop(self, parse_resultant: bool, parse_taxels: bool): """Background thread: acquire → offset → store → repeat. - - Error handling: - - FrameError: recoverable (bad LRC / parse error). Count and continue. - - IOError: serial-level hiccup. Count, back off briefly, continue. - - Any other Exception: unexpected (likely a programming bug). Log the - traceback and stop the stream loudly rather than rotting silently. """ while self._auto_running.is_set(): try: diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index ff2866a0..3c24b72d 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1147,10 +1147,8 @@ def stop_task(self): class OrcaHandTouch(OrcaHand): """ORCA hand with integrated tactile sensing. - Extends :class:`OrcaHand` to additionally manage a tactile sensor array. - Connection, disconnection, and lifecycle are unified: calling - :meth:`connect` opens both the motor bus and the sensor serial link, - and :meth:`disconnect` tears down both. + ``connect()`` opens both the motor bus and the sensor serial link; + ``disconnect()`` tears down both. """ config_cls = OrcaHandTouchConfig @@ -1321,12 +1319,7 @@ def _create_motor_client(self) -> MotorClient: class MockOrcaHandTouch(OrcaHandTouch): - """Drop-in :class:`OrcaHandTouch` backed by mock motor and sensor clients, - for testing and prototyping. - - All methods behave identically to :class:`OrcaHandTouch` but no serial - ports are opened and both motor and sensor state are simulated in memory. - """ + """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 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)",