diff --git a/benchmarks/audio/run_tactile_benchmark.py b/benchmarks/audio/run_tactile_benchmark.py index debfe1017..a10e8d606 100644 --- a/benchmarks/audio/run_tactile_benchmark.py +++ b/benchmarks/audio/run_tactile_benchmark.py @@ -20,8 +20,8 @@ from pathlib import Path import numpy as np import soundfile as sf +from lerobot.microphones.anyskin import AnyskinSensorConfig from lerobot.microphones.configs import MicrophoneConfig -from lerobot.microphones.touchlab import TouchLabSensorConfig from lerobot.microphones.utils import ( async_microphones_start_recording, async_microphones_stop_recording, @@ -120,9 +120,8 @@ if __name__ == "__main__": args["sensors_channels"], strict=False, ): - if isinstance(channels, int): - channels = [channels] - sensor_config = TouchLabSensorConfig( + channels = [1, 2, 3, 4, 5] + sensor_config = AnyskinSensorConfig( sensor_port=port, baud_rate=baud_rate, sample_rate=sample_rate, diff --git a/src/lerobot/__init__.py b/src/lerobot/__init__.py index 4be49f57c..a81eca1f6 100644 --- a/src/lerobot/__init__.py +++ b/src/lerobot/__init__.py @@ -179,6 +179,7 @@ available_cameras = [ available_microphones = [ "portaudio", "touchlab", + "anyskin", ] # lists all available motors from `lerobot/motors` diff --git a/src/lerobot/async_inference/robot_client.py b/src/lerobot/async_inference/robot_client.py index a21e23983..9cf6d74fd 100644 --- a/src/lerobot/async_inference/robot_client.py +++ b/src/lerobot/async_inference/robot_client.py @@ -47,6 +47,7 @@ import torch from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 +from lerobot.microphones.anyskin.configuration_anyskin import AnyskinSensorConfig # noqa: F401 from lerobot.microphones.portaudio.configuration_portaudio import PortAudioMicrophoneConfig # noqa: F401 from lerobot.microphones.touchlab.configuration_touchlab import TouchLabSensorConfig # noqa: F401 from lerobot.processor import RobotAction diff --git a/src/lerobot/microphones/anyskin/__init__.py b/src/lerobot/microphones/anyskin/__init__.py new file mode 100644 index 000000000..d6e1c88a0 --- /dev/null +++ b/src/lerobot/microphones/anyskin/__init__.py @@ -0,0 +1,16 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .configuration_anyskin import AnyskinSensorConfig +from .sensor_anyskin import AnyskinSensor diff --git a/src/lerobot/microphones/anyskin/configuration_anyskin.py b/src/lerobot/microphones/anyskin/configuration_anyskin.py new file mode 100644 index 000000000..b104138ff --- /dev/null +++ b/src/lerobot/microphones/anyskin/configuration_anyskin.py @@ -0,0 +1,45 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import dataclass + +from ..configs import MicrophoneConfig + + +@MicrophoneConfig.register_subclass("anyskin") +@dataclass +class AnyskinSensorConfig(MicrophoneConfig): + """Configuration class for Anyskin tactile sensors (technically not a microphone, but behaves like one acquisition-wise). + + This class provides configuration options for Anyskin tactile sensors, including serial port, sample rate and channels. + + Example configurations: + ```python + # Basic configurations + AnyskinSensorConfig("/dev/ttyACM0", 16000) # Serial port /dev/ttyACM0, 16000Hz + AnyskinSensorConfig("/dev/ttyACM1", 44100) # Serial port /dev/ttyACM1, 44100Hz + ``` + + Attributes: + sensor_port: Serial port of the tactile sensor. + baud_rate: Baud rate of the tactile sensor. + sample_rate: Sample rate in Hz for the tactile sensor. + channels: List of channel numbers to use for the tactile sensor. + """ + + sensor_port: str + baud_rate: int = 115_200 + sensor_id: int = 0 + burst_mode: bool = True + temp_filtered: bool = False diff --git a/src/lerobot/microphones/anyskin/sensor_anyskin.py b/src/lerobot/microphones/anyskin/sensor_anyskin.py new file mode 100644 index 000000000..944e31603 --- /dev/null +++ b/src/lerobot/microphones/anyskin/sensor_anyskin.py @@ -0,0 +1,473 @@ +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Provides the AnyskinSensor class for capturing tactile data from Anyskin tactile sensors. +""" + +from doctest import master +import logging +import time +from multiprocessing import ( + Event as process_Event, + JoinableQueue as process_Queue, + Process, +) +from pathlib import Path +from queue import Empty +from threading import Barrier, Event, Event as thread_Event, Thread +from typing import Any + +from lerobot.utils.hub import T +import numpy as np +from serial import Serial, serialutil +from soundfile import SoundFile + +from lerobot.utils.errors import ( + DeviceAlreadyConnectedError, + DeviceAlreadyRecordingError, + DeviceNotConnectedError, + DeviceNotRecordingError, +) +from lerobot.utils.shared_array import SharedArray + +from ..microphone import Microphone +from .configuration_anyskin import AnyskinSensorConfig + +from anyskin import AnySkinBase, AnySkinDummy + +logger = logging.getLogger(__name__) + +MAX_MAGNETS_CHANNELS = 5 + +class AnyskinSensor(Microphone): + """ + The AnyskinSensor class handles all Anyskin tactile sensors. + + A AnyskinSensor instance requires the serial port of the tactile sensor, which may be obtained using `python -m lerobot.find_port`. It also requires the recording sample rate as well as the list of recorded channels. + + Example of usage: + ```python + from lerobot.common.robot_devices.microphones.configs import AnyskinSensorConfig + + config = AnyskinSensorConfig(sensor_port="/dev/ttyACM0", baud_rate=115200, sample_rate=115, channels=[1]) + microphone = AnyskinSensor(config) + + microphone.connect() + microphone.start_recording("some/output/file.wav") + ... + audio_readings = microphone.read() # Gets all recorded audio data since the last read or since the beginning of the recording. The longer the period the longer the reading time ! + ... + microphone.stop_recording() + microphone.disconnect() + ``` + """ + + def __init__(self, config: AnyskinSensorConfig): + """ " + Initializes the AnyskinSensor instance. + + Args: + config: The configuration settings for the sensor. + """ + super().__init__(config) + + # Sensor port + self.sensor_port = config.sensor_port + + # Baud rate + self.baud_rate = config.baud_rate + + # Input audio recording process and events + self.record_process = None + self.record_stop_event = process_Event() + self.record_start_event = process_Event() + self.record_close_event = process_Event() + self.record_is_started_event = process_Event() + self.audio_callback_start_event = process_Event() + + # Process-safe concurrent queue to send audio from the recording process to the writing process/thread + self.write_queue = process_Queue() + + # SharedArray to store audio from the recording process. + self.read_shared_array = None + self.local_read_shared_array = None + # Thread/Process to handle data writing in a separate thread/process (safely) + self.write_thread = None + self.write_stop_event = None + self.write_is_started_event = None + + self.logs = {} + + def __str__(self) -> str: + return f"{self.__class__.__name__}({self.sensor_port})" + + @property + def is_connected(self) -> bool: + """Check if the sensor is currently connected. + + Returns: + bool: True if the sensor is connected and ready to start recording, + False otherwise. + """ + return self.record_process is not None and self.record_process.is_alive() + + @property + def is_recording(self) -> bool: + """Check if the sensor is currently recording. + + Returns: + bool: True if the sensor is recording, False otherwise. + """ + return self.record_is_started_event.is_set() + + @property + def is_writing(self) -> bool: + """Check if the sensor is currently writing to a file. + + Returns: + bool: True if the sensor is writing to a file, False otherwise. + """ + return self.write_thread is not None and self.write_is_started_event.is_set() + + @staticmethod + def find_microphones() -> list[dict[str, Any]]: + """Detects available sensors connected to the system. + + Returns: + List[Dict[str, Any]]: A list of dictionaries, + where each dictionary contains information about a detected sensor. + """ + pass + + def connect(self) -> None: + """ + Establish connection to the sensor. + """ + if self.is_connected: + raise DeviceAlreadyConnectedError(f"Sensor connected to {self.sensor_port} is already connected.") + + # Create or reset queue and shared array + self.read_shared_array = SharedArray( + shape=(self.sample_rate * 10, len(self.channels)), + dtype=np.dtype("int16"), + ) + self.local_read_shared_array = self.read_shared_array.get_local_array() + self.write_queue = process_Queue() + + # Reset events + self.record_start_event.clear() + self.record_stop_event.clear() + self.record_close_event.clear() + self.record_is_started_event.clear() + self.audio_callback_start_event.clear() + + # Create and start an audio input stream with a recording callback + # Remark: this is done in a separate process so that audio recording is not impacted by the main thread CPU usage, especially the busy_wait function. + process_init_event = process_Event() + self.record_process = Process( + target=self._record_process, + args=( + self.sensor_port, + self.baud_rate, + self.channels, + process_init_event, + self.record_start_event, + self.record_stop_event, + self.record_close_event, + self.record_is_started_event, + self.audio_callback_start_event, + self.write_queue, + self.read_shared_array, + ), + ) + self.record_process.daemon = True + self.record_process.start() + + is_init = process_init_event.wait( + timeout=5.0 + ) # Wait for the recording process to be started, and to potentially raise an error on failure. + if not self.is_connected or not is_init: + raise RuntimeError(f"Error connecting sensor connected to {self.sensor_port}.") + + logger.info(f"{self} connected.") + + @staticmethod + def _record_process( + sensor_port, + baud_rate, + channels, + process_init_event, + record_start_event, + record_stop_event, + record_close_event, + record_is_started_event, + audio_callback_start_event, + write_queue, + read_shared_array, + ) -> None: + channels_index = np.array(channels) - 1 + local_read_shared_array = read_shared_array.get_local_array() + + def tactile_callback(tactile_sensor: AnySkinBase): + """ + Parse the tactile data from the raw input data. + """ + if audio_callback_start_event.is_set(): + timestamp, indata = tactile_sensor.get_sample() + indata = indata.reshape(-1, MAX_MAGNETS_CHANNELS) + write_queue.put_nowait(indata[:, channels_index]) + read_shared_array.write(local_read_shared_array, indata[:, channels_index]) + + try: + tactile_sensor = AnySkinBase( + num_mags=MAX_MAGNETS_CHANNELS, + port=sensor_port, + baudrate=baud_rate, + burst_mode=True, + device_id=0, #TODO(CarolinePascal): create an abstract increasing id for each sensor + temp_filtered=False, + ) #TODO(CarolinePascal): add timeout on serial connection ? + except (serialutil.SerialException, AttributeError) as e: + raise RuntimeError(f"Error connecting sensor connected to {sensor_port}: {e}") + + process_init_event.set() + + while True: + start_flag = record_start_event.wait(timeout=0.1) + if record_close_event.is_set(): + break + elif not start_flag: + continue + record_is_started_event.set() + while not record_stop_event.is_set(): + tactile_callback(tactile_sensor) # Initial flush is already done in the constructor. + record_is_started_event.clear() + tactile_sensor.close() # Closes the inherited serial connection. + + def disconnect(self) -> None: + """ + Disconnect the sensor and release any resources. + """ + if not self.is_connected: + raise DeviceNotConnectedError(f"Sensor connected to {self.sensor_port} is not connected.") + + if self.is_recording: + self.stop_recording() + + self.record_close_event.set() + self.read_shared_array.delete() + self.write_queue.close() + self.record_process.join() + + if self.is_connected: + raise RuntimeError(f"Error disconnecting sensor connected to {self.sensor_port}.") + + logger.info(f"{self} disconnected.") + + def start_recording( + self, + output_file: str | Path | None = None, + multiprocessing: bool | None = False, + overwrite: bool | None = True, + barrier: Barrier | None = None, + ) -> None: + """ + Start recording tactile data from the sensor. + + Args: + output_file: Optional path to save the recorded tactile data. + multiprocessing: If True, enables multiprocessing for recording. Defaults to multithreading otherwise. + overwrite: If True, overwrites existing files at output_file path. + barrier: If not None, ensures that multiple sensors start recording at the same time. + """ + if not self.is_connected: + raise DeviceNotConnectedError(f"Sensor connected to {self.sensor_port} is not connected.") + if self.is_recording: + raise DeviceAlreadyRecordingError(f"Sensor connected to {self.sensor_port} is already recording.") + + # Reset queue and shared memory + self.read_shared_array.reset() + self._clear_queue(self.write_queue) + + # Reset stop event + self.record_stop_event.clear() + + # Write recordings into a file if output_file is provided + if output_file is not None: + output_file = Path(output_file) + output_file.parent.mkdir(parents=True, exist_ok=True) + + if output_file.exists(): + if overwrite: + output_file.unlink() + else: + raise FileExistsError( + f"Output file {output_file} already exists. Set overwrite to True to overwrite it." + ) + + if multiprocessing: + self.write_stop_event = process_Event() + self.write_is_started_event = process_Event() + self.write_thread = Process( + target=AnyskinSensor._write_loop, + args=( + self.write_queue, + self.write_stop_event, + self.write_is_started_event, + self.sample_rate, + self.channels, + output_file, + ), + ) + else: + self.write_stop_event = thread_Event() + self.write_is_started_event = thread_Event() + self.write_thread = Thread( + target=AnyskinSensor._write_loop, + args=( + self.write_queue, + self.write_stop_event, + self.write_is_started_event, + self.sample_rate, + self.channels, + output_file, + ), + ) + self.write_thread.daemon = True + self.write_thread.start() + self.write_is_started_event.wait() # Wait for the writing thread/process to be started. + + self.record_start_event.set() # Start the input audio stream process + self.record_is_started_event.wait() # Wait for the input audio stream process to be actually started + + if barrier is not None: + barrier.wait() # Wait for multiple input audio streams to be started at the same time + + self.audio_callback_start_event.set() + + if not self.is_recording: + raise RuntimeError(f"Error starting recording for sensor connected to {self.sensor_port}.") + if output_file is not None and not self.is_writing: + raise RuntimeError(f"Error starting writing for sensor connected to {self.sensor_port}.") + + def _read(self) -> np.ndarray: + """ + Thread/Process-safe callback to read available audio data + """ + return self.read_shared_array.read(self.local_read_shared_array, flush=True) + + def read(self) -> np.ndarray: + """Capture and return a single audio chunk from the sensor. + + Returns: + np.ndarray: Captured audio chunk as a numpy array. + """ + if not self.is_connected: + raise DeviceNotConnectedError(f"Sensor connected to {self.sensor_port} is not connected.") + if not self.is_recording: + raise RuntimeError(f"Sensor connected to {self.sensor_port} is not recording.") + + start_time = time.perf_counter() + + tactile_readings = self._read() + + # log the number of seconds it took to read the audio chunk + self.logs["delta_timestamp_s"] = time.perf_counter() - start_time + + # log the utc time at which the audio chunk was received + self.logs["timestamp_utc"] = time.perf_counter() + + return tactile_readings + + def _read_loop(self) -> None: + """Internal loop run by the background thread for asynchronous reading.""" + + def stop_recording(self) -> None: + """Stop recording audio from the sensor.""" + if not self.is_connected: + raise DeviceNotConnectedError(f"Sensor connected to {self.sensor_port} is not connected.") + if not self.is_recording: + raise DeviceNotRecordingError(f"Sensor connected to {self.sensor_port} is not recording.") + + self.audio_callback_start_event.clear() + self.record_start_event.clear() # Ensures the audio stream is not started again ! + self.record_stop_event.set() + + self.read_shared_array.reset() + self._clear_queue(self.write_queue, join_queue=True) + + if self.is_writing: + self.write_stop_event.set() + self.write_thread.join() + + timeout = 1.0 + while self.is_recording and timeout > 0: + time.sleep(0.01) + timeout -= 0.01 + + if self.is_recording: + raise RuntimeError(f"Error stopping recording for sensor connected to {self.sensor_port}.") + if self.is_writing: + raise RuntimeError(f"Error stopping writing for sensor connected to {self.sensor_port}.") + + def __del__(self) -> None: + if self.is_connected: + self.disconnect() + + @staticmethod + def _clear_queue(queue, join_queue: bool = False): + """ + Clears the queue by getting all items until it is empty. The longer the queue, the longer it takes to clear it. + """ + try: + while True: + queue.get_nowait() + queue.task_done() + except Empty: + if join_queue: + queue.join() + return + + @staticmethod + def _write_loop( + queue, + write_stop_event: Event, + write_is_started_event: Event, + sample_rate: int, + channels: list[int], + output_file: Path, + ) -> None: + """ + Thread/Process-safe loop to write audio data into a file. + """ + # Can only be run on a single process/thread for file writing safety + with SoundFile( + output_file, + mode="w", + samplerate=sample_rate, + channels=len(channels), + format="WAV", + subtype="FLOAT", # Subtype for float32 values + ) as file: + write_is_started_event.set() + while not write_stop_event.is_set(): + try: + file.write( + queue.get(timeout=0.005) + ) # Timeout set as the usual sounddevice buffer size. get_nowait is not possible here as it saturates the thread. + queue.task_done() + except Empty: + continue + write_is_started_event.clear() diff --git a/src/lerobot/microphones/utils.py b/src/lerobot/microphones/utils.py index cf19a970f..ab8bcebfd 100644 --- a/src/lerobot/microphones/utils.py +++ b/src/lerobot/microphones/utils.py @@ -31,6 +31,10 @@ def make_microphones_from_configs(microphone_configs: dict[str, MicrophoneConfig from .touchlab import TouchLabSensor microphones[key] = TouchLabSensor(cfg) + elif cfg.type == "anyskin": + from .anyskin import AnyskinSensor + + microphones[key] = AnyskinSensor(cfg) else: raise ValueError(f"The microphone type '{cfg.type}' is not valid.") diff --git a/src/lerobot/scripts/lerobot_calibrate.py b/src/lerobot/scripts/lerobot_calibrate.py index 2522486d8..7e68a142d 100644 --- a/src/lerobot/scripts/lerobot_calibrate.py +++ b/src/lerobot/scripts/lerobot_calibrate.py @@ -33,6 +33,7 @@ import draccus from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 +from lerobot.microphones.anyskin.configuration_anyskin import AnyskinSensorConfig # noqa: F401 from lerobot.microphones.portaudio.configuration_portaudio import PortAudioMicrophoneConfig # noqa: F401 from lerobot.microphones.touchlab.configuration_touchlab import TouchLabSensorConfig # noqa: F401 from lerobot.robots import ( # noqa: F401 diff --git a/src/lerobot/scripts/lerobot_record.py b/src/lerobot/scripts/lerobot_record.py index e01a30957..94d261888 100644 --- a/src/lerobot/scripts/lerobot_record.py +++ b/src/lerobot/scripts/lerobot_record.py @@ -94,6 +94,7 @@ from lerobot.datasets.video_utils import VideoEncodingManager from lerobot.microphones import ( MicrophoneConfig, # noqa: F401 ) +from lerobot.microphones.anyskin.configuration_anyskin import AnyskinSensorConfig # noqa: F401 from lerobot.microphones.portaudio.configuration_portaudio import PortAudioMicrophoneConfig # noqa: F401 from lerobot.microphones.touchlab.configuration_touchlab import TouchLabSensorConfig # noqa: F401 from lerobot.microphones.utils import ( diff --git a/src/lerobot/scripts/lerobot_teleoperate.py b/src/lerobot/scripts/lerobot_teleoperate.py index 367accce8..b3768ff1f 100644 --- a/src/lerobot/scripts/lerobot_teleoperate.py +++ b/src/lerobot/scripts/lerobot_teleoperate.py @@ -61,6 +61,7 @@ import rerun as rr from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401 from lerobot.configs import parser +from lerobot.microphones.anyskin.configuration_anyskin import AnyskinSensorConfig # noqa: F401 from lerobot.microphones.portaudio.configuration_portaudio import PortAudioMicrophoneConfig # noqa: F401 from lerobot.microphones.touchlab.configuration_touchlab import TouchLabSensorConfig # noqa: F401 from lerobot.processor import (