hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests Activity

Merge branch 'user/aliberts/2025_02_25_refactor_robots' into refactor/updated_api_docs

Browse Source
This commit is contained in:
Pepijn
2025-05-15 12:46:56 +02:00
committed by Steven Palma
parent 859a369b29 65ad0650b0
commit 3c1b657fdd
39 changed files with 780 additions and 967 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
.cache/calibration/aloha_default/left_follower.json
View File

@@ -1,68 +0,0 @@
{
"homing_offset": [
2048,
3072,
3072,
-1024,
-1024,
2048,
-2048,
2048,
-2048
],
"drive_mode": [
1,
1,
1,
0,
0,
1,
0,
1,
0
],
"start_pos": [
2015,
3058,
3061,
1071,
1071,
2035,
2152,
2029,
2499
],
"end_pos": [
-1008,
-1963,
-1966,
2141,
2143,
-971,
3043,
-1077,
3144
],
"calib_mode": [
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"LINEAR"
],
"motor_names": [
"waist",
"shoulder",
"shoulder_shadow",
"elbow",
"elbow_shadow",
"forearm_roll",
"wrist_angle",
"wrist_rotate",
"gripper"
]
}

68
.cache/calibration/aloha_default/left_leader.json
View File

@@ -1,68 +0,0 @@
{
"homing_offset": [
2048,
3072,
3072,
-1024,
-1024,
2048,
-2048,
2048,
-1024
],
"drive_mode": [
1,
1,
1,
0,
0,
1,
0,
1,
0
],
"start_pos": [
2035,
3024,
3019,
979,
981,
1982,
2166,
2124,
1968
],
"end_pos": [
-990,
-2017,
-2015,
2078,
2076,
-1030,
3117,
-1016,
2556
],
"calib_mode": [
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"LINEAR"
],
"motor_names": [
"waist",
"shoulder",
"shoulder_shadow",
"elbow",
"elbow_shadow",
"forearm_roll",
"wrist_angle",
"wrist_rotate",
"gripper"
]
}

68
.cache/calibration/aloha_default/right_follower.json
View File

@@ -1,68 +0,0 @@
{
"homing_offset": [
2048,
3072,
3072,
-1024,
-1024,
2048,
-2048,
2048,
-2048
],
"drive_mode": [
1,
1,
1,
0,
0,
1,
0,
1,
0
],
"start_pos": [
2056,
2895,
2896,
1191,
1190,
2018,
2051,
2056,
2509
],
"end_pos": [
-1040,
-2004,
-2006,
2126,
2127,
-1010,
3050,
-1117,
3143
],
"calib_mode": [
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"LINEAR"
],
"motor_names": [
"waist",
"shoulder",
"shoulder_shadow",
"elbow",
"elbow_shadow",
"forearm_roll",
"wrist_angle",
"wrist_rotate",
"gripper"
]
}

68
.cache/calibration/aloha_default/right_leader.json
View File

@@ -1,68 +0,0 @@
{
"homing_offset": [
2048,
3072,
3072,
-1024,
-1024,
2048,
-2048,
2048,
-2048
],
"drive_mode": [
1,
1,
1,
0,
0,
1,
0,
1,
0
],
"start_pos": [
2068,
3034,
3030,
1038,
1041,
1991,
1948,
2090,
1985
],
"end_pos": [
-1025,
-2014,
-2015,
2058,
2060,
-955,
3091,
-940,
2576
],
"calib_mode": [
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"DEGREE",
"LINEAR"
],
"motor_names": [
"waist",
"shoulder",
"shoulder_shadow",
"elbow",
"elbow_shadow",
"forearm_roll",
"wrist_angle",
"wrist_rotate",
"gripper"
]
}

2
.gitattributes vendored
View File

@@ -18,4 +18,4 @@
*.arrow filter=lfs diff=lfs merge=lfs -text *.arrow filter=lfs diff=lfs merge=lfs -text
*.json !text !filter !merge !diff *.json !text !filter !merge !diff
tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text tests/artifacts/cameras/*.png filter=lfs diff=lfs merge=lfs -text
tests/artifacts/cameras/*.bag filter=lfs diff=lfs merge=lfs -text *.bag filter=lfs diff=lfs merge=lfs -text

7
.gitignore vendored
View File

@@ -11,7 +11,10 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
# Dev scripts
.dev .dev
# Logging # Logging
logs logs
tmp tmp
@@ -91,10 +94,8 @@ coverage.xml
.hypothesis/ .hypothesis/
.pytest_cache/ .pytest_cache/
# Ignore .cache except calibration # Ignore .cache
.cache/* .cache/*
!.cache/calibration/
!.cache/calibration/**
# Translations # Translations
*.mo *.mo

2
lerobot/calibrate.py
View File

@@ -31,6 +31,8 @@ from pprint import pformat
import draccus import draccus
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401
from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
from lerobot.common.robots import ( # noqa: F401 from lerobot.common.robots import ( # noqa: F401
Robot, Robot,
RobotConfig, RobotConfig,

2
lerobot/common/cameras/__init__.py
View File

@@ -13,5 +13,5 @@
# limitations under the License. # limitations under the License.
from .camera import Camera from .camera import Camera
from .configs import CameraConfig from .configs import CameraConfig, ColorMode, Cv2Rotation
from .utils import make_cameras_from_configs from .utils import make_cameras_from_configs

77
lerobot/common/cameras/camera.py
View File

@@ -15,6 +15,7 @@
# limitations under the License. # limitations under the License.
import abc import abc
from typing import Any, Dict, List
import numpy as np import numpy as np
@@ -22,28 +23,100 @@ from .configs import CameraConfig, ColorMode
class Camera(abc.ABC): class Camera(abc.ABC):
"""Base class for camera implementations.
Defines a standard interface for camera operations across different backends.
Subclasses must implement all abstract methods.
Manages basic camera properties (FPS, resolution) and core operations:
- Connection/disconnection
- Frame capture (sync/async)
Attributes:
fps (int | None): Configured frames per second
width (int | None): Frame width in pixels
height (int | None): Frame height in pixels
warmup_time (int | None): Time reading frames before returning from connect (in seconds)
Example:
class MyCamera(Camera):
def __init__(self, config): ...
@property
def is_connected(self) -> bool: ...
def connect(self, warmup=True): ...
# Plus other required methods
"""
def __init__(self, config: CameraConfig): def __init__(self, config: CameraConfig):
"""Initialize the camera with the given configuration.
Args:
config: Camera configuration containing FPS and resolution.
"""
self.fps: int | None = config.fps self.fps: int | None = config.fps
self.width: int | None = config.width self.width: int | None = config.width
self.height: int | None = config.height self.height: int | None = config.height
self.warmup_time: int | None = config.warmup_time
@property @property
@abc.abstractmethod @abc.abstractmethod
def is_connected(self) -> bool: def is_connected(self) -> bool:
"""Check if the camera is currently connected.
Returns:
bool: True if the camera is connected and ready to capture frames,
False otherwise.
"""
pass
@staticmethod
@abc.abstractmethod
def find_cameras() -> List[Dict[str, Any]]:
"""Detects available cameras connected to the system.
Returns:
List[Dict[str, Any]]: A list of dictionaries,
where each dictionary contains information about a detected camera.
"""
pass pass
@abc.abstractmethod @abc.abstractmethod
def connect(self, do_warmup_read: bool = True) -> None: def connect(self, warmup: bool = True) -> None:
"""Establish connection to the camera.
Args:
warmup: If True (default), captures a warmup frame before returning. Useful
for cameras that require time to adjust capture settings.
If False, skips the warmup frame.
"""
pass pass
@abc.abstractmethod @abc.abstractmethod
def read(self, color_mode: ColorMode | None = None) -> np.ndarray: def read(self, color_mode: ColorMode | None = None) -> np.ndarray:
"""Capture and return a single frame from the camera.
Args:
color_mode: Desired color mode for the output frame. If None,
uses the camera's default color mode.
Returns:
np.ndarray: Captured frame as a numpy array.
"""
pass pass
@abc.abstractmethod @abc.abstractmethod
def async_read(self, timeout_ms: float = 2000) -> np.ndarray: def async_read(self, timeout_ms: float = ...) -> np.ndarray:
"""Asynchronously capture and return a single frame from the camera.
Args:
timeout_ms: Maximum time to wait for a frame in milliseconds.
Defaults to implementation-specific timeout.
Returns:
np.ndarray: Captured frame as a numpy array.
"""
pass pass
@abc.abstractmethod @abc.abstractmethod
def disconnect(self) -> None: def disconnect(self) -> None:
"""Disconnect from the camera and release resources."""
pass pass

5
lerobot/common/cameras/configs.py
View File

@@ -21,12 +21,12 @@ from enum import Enum
import draccus import draccus
class ColorMode(Enum): class ColorMode(str, Enum):
RGB = "rgb" RGB = "rgb"
BGR = "bgr" BGR = "bgr"
class Cv2Rotation(Enum): class Cv2Rotation(int, Enum):
NO_ROTATION = 0 NO_ROTATION = 0
ROTATE_90 = 90 ROTATE_90 = 90
ROTATE_180 = 180 ROTATE_180 = 180
@@ -38,6 +38,7 @@ class CameraConfig(draccus.ChoiceRegistry, abc.ABC):
fps: int | None = None fps: int | None = None
width: int | None = None width: int | None = None
height: int | None = None height: int | None = None
warmup_time: int | None = None
@property @property
def type(self) -> str: def type(self) -> str:

278
lerobot/common/cameras/opencv/camera_opencv.py
View File

@@ -30,10 +30,9 @@ import cv2
import numpy as np import numpy as np
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.utils.utils import capture_timestamp_utc
from ..camera import Camera from ..camera import Camera
from ..utils import IndexOrPath, get_cv2_backend, get_cv2_rotation from ..utils import get_cv2_backend, get_cv2_rotation
from .configuration_opencv import ColorMode, OpenCVCameraConfig from .configuration_opencv import ColorMode, OpenCVCameraConfig
# NOTE(Steven): The maximum opencv device index depends on your operating system. For instance, # NOTE(Steven): The maximum opencv device index depends on your operating system. For instance,
@@ -59,20 +58,16 @@ class OpenCVCamera(Camera):
or port changes, especially on Linux. Use the provided utility script to find or port changes, especially on Linux. Use the provided utility script to find
available camera indices or paths: available camera indices or paths:
```bash ```bash
python -m lerobot.find_cameras python -m lerobot.find_cameras opencv
``` ```
The camera's default settings (FPS, resolution, color mode) are used unless The camera's default settings (FPS, resolution, color mode) are used unless
overridden in the configuration. overridden in the configuration.
Args:
config (OpenCVCameraConfig): Configuration object containing settings like
camera index/path, desired FPS, width, height, color mode, and rotation.
Example: Example:
```python ```python
from lerobot.common.cameras.opencv import OpenCVCamera from lerobot.common.cameras.opencv import OpenCVCamera
from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode from lerobot.common.cameras.configuration_opencv import OpenCVCameraConfig, ColorMode, Cv2Rotation
# Basic usage with camera index 0 # Basic usage with camera index 0
config = OpenCVCameraConfig(index_or_path=0) config = OpenCVCameraConfig(index_or_path=0)
@@ -97,7 +92,7 @@ class OpenCVCamera(Camera):
width=1280, width=1280,
height=720, height=720,
color_mode=ColorMode.RGB, color_mode=ColorMode.RGB,
rotation=90 rotation=Cv2Rotation.ROTATE_90
) )
custom_camera = OpenCVCamera(custom_config) custom_camera = OpenCVCamera(custom_config)
# ... connect, read, disconnect ... # ... connect, read, disconnect ...
@@ -114,37 +109,79 @@ class OpenCVCamera(Camera):
super().__init__(config) super().__init__(config)
self.config = config self.config = config
self.index_or_path: IndexOrPath = config.index_or_path self.index_or_path = config.index_or_path
self.fps: int | None = config.fps self.fps = config.fps
self.channels: int = config.channels self.color_mode = config.color_mode
self.color_mode: ColorMode = config.color_mode self.warmup_time = config.warmup_time
self.videocapture_camera: cv2.VideoCapture | None = None self.videocapture: cv2.VideoCapture | None = None
self.thread: Thread | None = None self.thread: Thread | None = None
self.stop_event: Event | None = None self.stop_event: Event | None = None
self.frame_queue: queue.Queue = queue.Queue(maxsize=1) self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
self.logs: dict = {} # NOTE(Steven): Might be removed in the future
self.rotation: int | None = get_cv2_rotation(config.rotation) self.rotation: int | None = get_cv2_rotation(config.rotation)
self.backend: int = get_cv2_backend() # NOTE(Steven): If we specify backend the opencv open fails self.backend: int = get_cv2_backend()
if self.height and self.width: if self.height and self.width:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.prerotated_width, self.prerotated_height = self.height, self.width self.capture_width, self.capture_height = self.height, self.width
else: else:
self.prerotated_width, self.prerotated_height = self.width, self.height self.capture_width, self.capture_height = self.width, self.height
def __str__(self) -> str: def __str__(self) -> str:
"""Returns a string representation of the camera instance."""
return f"{self.__class__.__name__}({self.index_or_path})" return f"{self.__class__.__name__}({self.index_or_path})"
@property @property
def is_connected(self) -> bool: def is_connected(self) -> bool:
"""Checks if the camera is currently connected and opened.""" """Checks if the camera is currently connected and opened."""
return isinstance(self.videocapture_camera, cv2.VideoCapture) and self.videocapture_camera.isOpened() return isinstance(self.videocapture, cv2.VideoCapture) and self.videocapture.isOpened()
def connect(self, warmup: bool = True):
"""
Connects to the OpenCV camera specified in the configuration.
Initializes the OpenCV VideoCapture object, sets desired camera properties
(FPS, width, height), and performs initial checks.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the specified camera index/path is not found or accessible.
ConnectionError: If the camera is found but fails to open.
RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
# Use 1 thread for OpenCV operations to avoid potential conflicts or
# blocking in multi-threaded applications, especially during data collection.
cv2.setNumThreads(1)
self.videocapture = cv2.VideoCapture(self.index_or_path, self.backend)
if not self.videocapture.isOpened():
self.videocapture.release()
self.videocapture = None
raise ConnectionError(
f"Failed to open OpenCV camera {self.index_or_path}."
f"Run 'python -m lerobot.find_cameras opencv' for details about the available cameras in your system."
)
self._configure_capture_settings()
if warmup:
if self.warmup_time is None:
raise ValueError(
f"Warmup time is not set for {self}. Please set a warmup time in the configuration."
)
logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...")
start_time = time.time()
while time.time() - start_time < self.warmup_time:
self.read()
time.sleep(0.1)
logger.debug(f"Camera {self.index_or_path} connected and configured successfully.")
def _configure_capture_settings(self) -> None: def _configure_capture_settings(self) -> None:
""" """
@@ -167,120 +204,65 @@ class OpenCVCamera(Camera):
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.") raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.")
self._validate_fps() if self.fps is None:
self._validate_width_and_height() self.fps = self.videocapture.get(cv2.CAP_PROP_FPS)
logger.info(f"FPS set to camera default: {self.fps}.")
else:
self._validate_fps()
def connect(self, do_warmup_read: bool = True): default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
""" default_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
Connects to the OpenCV camera specified in the configuration.
Initializes the OpenCV VideoCapture object, sets desired camera properties if self.width is None or self.height is None:
(FPS, width, height), and performs initial checks. if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = default_height, default_width
Raises: self.capture_width, self.capture_height = default_width, default_height
DeviceAlreadyConnectedError: If the camera is already connected. else:
ValueError: If the specified camera index/path is not found or accessible. self.width, self.height = default_width, default_height
ConnectionError: If the camera is found but fails to open. self.capture_width, self.capture_height = default_width, default_height
RuntimeError: If the camera opens but fails to apply requested FPS/resolution settings. logger.info(f"Capture width set to camera default: {self.width}.")
""" logger.info(f"Capture height set to camera default: {self.height}.")
if self.is_connected: else:
raise DeviceAlreadyConnectedError(f"{self} is already connected.") self._validate_width_and_height()
# Use 1 thread for OpenCV operations to avoid potential conflicts or
# blocking in multi-threaded applications, especially during data collection.
cv2.setNumThreads(1)
logger.debug(f"Attempting to connect to camera {self.index_or_path} using backend {self.backend}...")
self.videocapture_camera = cv2.VideoCapture(self.index_or_path)
if not self.videocapture_camera.isOpened():
self.videocapture_camera.release()
self.videocapture_camera = None
raise ConnectionError(
f"Failed to open OpenCV camera {self.index_or_path}."
f"Run 'python -m find_cameras list-cameras' for details."
)
logger.debug(f"Successfully opened camera {self.index_or_path}. Applying configuration...")
self._configure_capture_settings()
if do_warmup_read:
logger.debug(f"Reading a warm-up frame for {self.index_or_path}...")
self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
logger.debug(f"Camera {self.index_or_path} connected and configured successfully.")
def _validate_fps(self) -> None: def _validate_fps(self) -> None:
"""Validates and sets the camera's frames per second (FPS).""" """Validates and sets the camera's frames per second (FPS)."""
if self.fps is None: success = self.videocapture.set(cv2.CAP_PROP_FPS, float(self.fps))
self.fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS) actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS)
logger.info(f"FPS set to camera default: {self.fps}.")
return
success = self.videocapture_camera.set(cv2.CAP_PROP_FPS, float(self.fps))
actual_fps = self.videocapture_camera.get(cv2.CAP_PROP_FPS)
# Use math.isclose for robust float comparison # Use math.isclose for robust float comparison
if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3): if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
logger.warning(
f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps} (set success: {success}). "
"This might be due to camera limitations."
)
raise RuntimeError( raise RuntimeError(
f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}." f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps} set success: {success})."
) )
logger.debug(f"FPS set to {actual_fps} for {self}.") logger.debug(f"FPS set to {actual_fps} for {self}.")
def _validate_width_and_height(self) -> None: def _validate_width_and_height(self) -> None:
"""Validates and sets the camera's frame capture width and height.""" """Validates and sets the camera's frame capture width and height."""
actual_width = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_WIDTH))) success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.capture_width))
actual_height = int(round(self.videocapture_camera.get(cv2.CAP_PROP_FRAME_HEIGHT))) actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
if not success or self.capture_width != actual_width:
if self.width is None or self.height is None:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = actual_height, actual_width
self.prerotated_width, self.prerotated_height = actual_width, actual_height
else:
self.width, self.height = actual_width, actual_height
self.prerotated_width, self.prerotated_height = actual_width, actual_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
return
success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.prerotated_width))
if not success or self.prerotated_width != actual_width:
logger.warning(
f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width} (set success: {success})."
)
raise RuntimeError( raise RuntimeError(
f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}." f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width} (set success: {success})."
) )
logger.debug(f"Capture width set to {actual_width} for {self}.") logger.debug(f"Capture width set to {actual_width} for {self}.")
success = self.videocapture_camera.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.prerotated_height)) success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.capture_height))
if not success or self.prerotated_height != actual_height: actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
logger.warning( if not success or self.capture_height != actual_height:
f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height} (set success: {success})."
)
raise RuntimeError( raise RuntimeError(
f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}." f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height} (set success: {success})."
) )
logger.debug(f"Capture height set to {actual_height} for {self}.") logger.debug(f"Capture height set to {actual_height} for {self}.")
@staticmethod @staticmethod
def find_cameras( def find_cameras() -> List[Dict[str, Any]]:
max_index_search_range=MAX_OPENCV_INDEX, raise_when_empty: bool = True
) -> List[Dict[str, Any]]:
""" """
Detects available OpenCV cameras connected to the system. Detects available OpenCV cameras connected to the system.
On Linux, it scans '/dev/video*' paths. On other systems (like macOS, Windows), On Linux, it scans '/dev/video*' paths. On other systems (like macOS, Windows),
it checks indices from 0 up to `max_index_search_range`. it checks indices from 0 up to `MAX_OPENCV_INDEX`.
Args:
max_index_search_range (int): The maximum index to check on non-Linux systems.
raise_when_empty (bool): If True, raises an OSError if no cameras are found.
Returns: Returns:
List[Dict[str, Any]]: A list of dictionaries, List[Dict[str, Any]]: A list of dictionaries,
@@ -290,16 +272,12 @@ class OpenCVCamera(Camera):
found_cameras_info = [] found_cameras_info = []
if platform.system() == "Linux": if platform.system() == "Linux":
logger.info("Linux detected. Scanning '/dev/video*' device paths...")
possible_paths = sorted(Path("/dev").glob("video*"), key=lambda p: p.name) possible_paths = sorted(Path("/dev").glob("video*"), key=lambda p: p.name)
targets_to_scan = [str(p) for p in possible_paths] targets_to_scan = [str(p) for p in possible_paths]
logger.debug(f"Found potential paths: {targets_to_scan}")
else: else:
logger.info( targets_to_scan = list(range(MAX_OPENCV_INDEX))
f"{platform.system()} system detected. Scanning indices from 0 to {max_index_search_range}..."
)
targets_to_scan = list(range(max_index_search_range))
logger.debug(f"Found potential paths: {targets_to_scan}")
for target in targets_to_scan: for target in targets_to_scan:
camera = cv2.VideoCapture(target) camera = cv2.VideoCapture(target)
if camera.isOpened(): if camera.isOpened():
@@ -321,13 +299,10 @@ class OpenCVCamera(Camera):
} }
found_cameras_info.append(camera_info) found_cameras_info.append(camera_info)
logger.debug(f"Found OpenCV camera:: {camera_info}")
camera.release() camera.release()
if not found_cameras_info: if not found_cameras_info:
logger.warning("No OpenCV devices detected.") logger.warning("No OpenCV devices detected.")
if raise_when_empty:
raise OSError("No OpenCV devices detected. Ensure cameras are connected.")
logger.info(f"Detected OpenCV cameras: {[cam['id'] for cam in found_cameras_info]}") logger.info(f"Detected OpenCV cameras: {[cam['id'] for cam in found_cameras_info]}")
return found_cameras_info return found_cameras_info
@@ -361,7 +336,7 @@ class OpenCVCamera(Camera):
start_time = time.perf_counter() start_time = time.perf_counter()
# NOTE(Steven): Are we okay with this blocking an undefined amount of time? # NOTE(Steven): Are we okay with this blocking an undefined amount of time?
ret, frame = self.videocapture_camera.read() ret, frame = self.videocapture.read()
if not ret or frame is None: if not ret or frame is None:
raise RuntimeError( raise RuntimeError(
@@ -374,7 +349,6 @@ class OpenCVCamera(Camera):
read_duration_ms = (time.perf_counter() - start_time) * 1e3 read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return processed_frame return processed_frame
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray: def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
@@ -403,13 +377,9 @@ class OpenCVCamera(Camera):
h, w, c = image.shape h, w, c = image.shape
if h != self.prerotated_height or w != self.prerotated_width: if h != self.capture_height or w != self.capture_width:
raise RuntimeError( raise RuntimeError(
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}." f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}."
)
if c != self.channels:
logger.warning(
f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
) )
processed_image = image processed_image = image
@@ -449,7 +419,7 @@ class OpenCVCamera(Camera):
logger.debug(f"Stopping read loop thread for {self}.") logger.debug(f"Stopping read loop thread for {self}.")
def _ensure_read_thread_running(self): def _start_read_thread(self) -> None:
"""Starts or restarts the background read thread if it's not running.""" """Starts or restarts the background read thread if it's not running."""
if self.thread is not None and self.thread.is_alive(): if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=0.1) self.thread.join(timeout=0.1)
@@ -464,6 +434,22 @@ class OpenCVCamera(Camera):
self.thread.start() self.thread.start()
logger.debug(f"Read thread started for {self}.") logger.debug(f"Read thread started for {self}.")
def _stop_read_thread(self) -> None:
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
logger.debug(f"Read thread stopped for {self}.")
def async_read(self, timeout_ms: float = 2000) -> np.ndarray: def async_read(self, timeout_ms: float = 2000) -> np.ndarray:
""" """
Reads the latest available frame asynchronously. Reads the latest available frame asynchronously.
@@ -490,41 +476,20 @@ class OpenCVCamera(Camera):
raise DeviceNotConnectedError(f"{self} is not connected.") raise DeviceNotConnectedError(f"{self} is not connected.")
if self.thread is None or not self.thread.is_alive(): if self.thread is None or not self.thread.is_alive():
self._ensure_read_thread_running() self._start_read_thread()
try: try:
return self.frame_queue.get(timeout=timeout_ms / 1000.0) return self.frame_queue.get(timeout=timeout_ms / 1000.0)
except queue.Empty as e: except queue.Empty as e:
thread_alive = self.thread is not None and self.thread.is_alive() thread_alive = self.thread is not None and self.thread.is_alive()
logger.error(
f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
f"(Read thread alive: {thread_alive})"
)
raise TimeoutError( raise TimeoutError(
f"Timed out waiting for frame from camera {self.index_or_path} after {timeout_ms} ms. " f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. "
f"Read thread alive: {thread_alive}." f"Read thread alive: {thread_alive}."
) from e ) from e
except Exception as e: except Exception as e:
logger.exception(f"Unexpected error getting frame from queue for {self}: {e}") logger.exception(f"Unexpected error getting frame from queue for {self}: {e}")
raise RuntimeError(f"Error getting frame from queue for camera {self.index_or_path}: {e}") from e raise RuntimeError(f"Error getting frame from queue for camera {self.index_or_path}: {e}") from e
def _shutdown_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
logger.debug(f"Signaling stop event for read thread of {self}.")
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
logger.debug(f"Waiting for read thread of {self} to join...")
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
def disconnect(self): def disconnect(self):
""" """
Disconnects from the camera and cleans up resources. Disconnects from the camera and cleans up resources.
@@ -536,18 +501,13 @@ class OpenCVCamera(Camera):
DeviceNotConnectedError: If the camera is already disconnected. DeviceNotConnectedError: If the camera is already disconnected.
""" """
if not self.is_connected and self.thread is None: if not self.is_connected and self.thread is None:
raise DeviceNotConnectedError( raise DeviceNotConnectedError(f"{self} not connected.")
f"Attempted to disconnect {self}, but it appears already disconnected."
)
logger.debug(f"Disconnecting from camera {self.index_or_path}...")
if self.thread is not None: if self.thread is not None:
self._shutdown_read_thread() self._stop_read_thread()
if self.videocapture_camera is not None: if self.videocapture is not None:
logger.debug(f"Releasing OpenCV VideoCapture object for {self}.") self.videocapture.release()
self.videocapture_camera.release() self.videocapture = None
self.videocapture_camera = None
logger.info(f"Camera {self.index_or_path} disconnected successfully.") logger.info(f"{self} disconnected.")

5
lerobot/common/cameras/opencv/configuration_opencv.py
View File

@@ -44,7 +44,6 @@ class OpenCVCameraConfig(CameraConfig):
width: Requested frame width in pixels for the color stream. width: Requested frame width in pixels for the color stream.
height: Requested frame height in pixels for the color stream. height: Requested frame height in pixels for the color stream.
color_mode: Color mode for image output (RGB or BGR). Defaults to RGB. color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
channels: Number of color channels (currently only 3 is supported).
rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
Note: Note:
@@ -53,7 +52,6 @@ class OpenCVCameraConfig(CameraConfig):
index_or_path: int | Path index_or_path: int | Path
color_mode: ColorMode = ColorMode.RGB color_mode: ColorMode = ColorMode.RGB
channels: int = 3 # NOTE(Steven): Why is this a config?
rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION
def __post_init__(self): def __post_init__(self):
@@ -71,6 +69,3 @@ class OpenCVCameraConfig(CameraConfig):
raise ValueError( raise ValueError(
f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided." f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
) )
if self.channels != 3:
raise NotImplementedError(f"Unsupported number of channels: {self.channels}")

0
lerobot/common/cameras/intel/__init__.py → lerobot/common/cameras/realsense/__init__.py
View File

364
lerobot/common/cameras/intel/camera_realsense.py → lerobot/common/cameras/realsense/camera_realsense.py
View File

@@ -18,7 +18,6 @@ Provides the RealSenseCamera class for capturing frames from Intel RealSense cam
import contextlib import contextlib
import logging import logging
import math
import queue import queue
import time import time
from threading import Event, Thread from threading import Event, Thread
@@ -26,10 +25,13 @@ from typing import Any, Dict, List
import cv2 import cv2
import numpy as np import numpy as np
import pyrealsense2 as rs
try:
import pyrealsense2 as rs
except Exception as e:
logging.info(f"Could not import realsense: {e}")
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
from lerobot.common.utils.utils import capture_timestamp_utc
from ..camera import Camera from ..camera import Camera
from ..configs import ColorMode from ..configs import ColorMode
@@ -51,7 +53,7 @@ class RealSenseCamera(Camera):
Use the provided utility script to find available camera indices and default profiles: Use the provided utility script to find available camera indices and default profiles:
```bash ```bash
python -m lerobot.find_cameras python -m lerobot.find_cameras realsense
``` ```
A `RealSenseCamera` instance requires a configuration object specifying the A `RealSenseCamera` instance requires a configuration object specifying the
@@ -61,53 +63,46 @@ class RealSenseCamera(Camera):
The camera's default settings (FPS, resolution, color mode) from the stream The camera's default settings (FPS, resolution, color mode) from the stream
profile are used unless overridden in the configuration. profile are used unless overridden in the configuration.
Args:
config (RealSenseCameraConfig): Configuration object containing settings like
serial number or name, desired FPS, width, height, color mode, rotation,
and whether to capture depth.
Example: Example:
```python ```python
from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig from lerobot.common.cameras import ColorMode, Cv2Rotation
from lerobot.common.cameras.configs import ColorMode
# Basic usage with serial number # Basic usage with serial number
config = RealSenseCameraConfig(serial_number="1234567890") # Replace with actual SN config = RealSenseCameraConfig(serial_number_or_name="1234567890") # Replace with actual SN
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
try: camera.connect()
camera.connect()
print(f"Connected to {camera}") # Read 1 frame synchronously
color_image = camera.read() # Synchronous read (color only) color_image = camera.read()
print(f"Read frame shape: {color_image.shape}") print(color_image.shape)
async_image = camera.async_read() # Asynchronous read
print(f"Async read frame shape: {async_image.shape}") # Read 1 frame asynchronously
except Exception as e: async_image = camera.async_read()
print(f"An error occurred: {e}")
finally: # When done, properly disconnect the camera using
camera.disconnect() camera.disconnect()
print(f"Disconnected from {camera}")
# Example with depth capture and custom settings # Example with depth capture and custom settings
custom_config = RealSenseCameraConfig( custom_config = RealSenseCameraConfig(
serial_number="1234567890", # Replace with actual SN serial_number_or_name="1234567890", # Replace with actual SN
fps=30, fps=30,
width=1280, width=1280,
height=720, height=720,
color_mode=ColorMode.BGR, # Request BGR output color_mode=ColorMode.BGR, # Request BGR output
rotation=0, rotation=Cv2Rotation.NO_ROTATION,
use_depth=True use_depth=True
) )
depth_camera = RealSenseCamera(custom_config) depth_camera = RealSenseCamera(custom_config)
try: try:
depth_camera.connect() depth_camera.connect()
color_image, depth_map = depth_camera.read() # Returns tuple depth_map = depth_camera.read_depth()
print(f"Color shape: {color_image.shape}, Depth shape: {depth_map.shape}") print(f"Depth shape: {depth_map.shape}")
finally: finally:
depth_camera.disconnect() depth_camera.disconnect()
# Example using a unique camera name # Example using a unique camera name
name_config = RealSenseCameraConfig(name="Intel RealSense D435") # If unique name_config = RealSenseCameraConfig(serial_number_or_name="Intel RealSense D435") # If unique
name_camera = RealSenseCamera(name_config) name_camera = RealSenseCamera(name_config)
# ... connect, read, disconnect ... # ... connect, read, disconnect ...
``` ```
@@ -125,17 +120,15 @@ class RealSenseCamera(Camera):
self.config = config self.config = config
if config.name is not None: # NOTE(Steven): Do we want to continue supporting this? if isinstance(config.serial_number_or_name, int):
self.serial_number = self._find_serial_number_from_name(config.name) self.serial_number = str(config.serial_number_or_name)
elif config.serial_number is not None:
self.serial_number = str(config.serial_number)
else: else:
raise ValueError("RealSenseCameraConfig must provide either 'serial_number' or 'name'.") self.serial_number = self._find_serial_number_from_name(config.serial_number_or_name)
self.fps: int | None = config.fps self.fps = config.fps
self.channels: int = config.channels self.color_mode = config.color_mode
self.color_mode: ColorMode = config.color_mode self.use_depth = config.use_depth
self.use_depth: bool = config.use_depth self.warmup_time = config.warmup_time
self.rs_pipeline: rs.pipeline | None = None self.rs_pipeline: rs.pipeline | None = None
self.rs_profile: rs.pipeline_profile | None = None self.rs_profile: rs.pipeline_profile | None = None
@@ -144,18 +137,15 @@ class RealSenseCamera(Camera):
self.stop_event: Event | None = None self.stop_event: Event | None = None
self.frame_queue: queue.Queue = queue.Queue(maxsize=1) self.frame_queue: queue.Queue = queue.Queue(maxsize=1)
self.logs: dict = {} # For timestamping or other metadata
self.rotation: int | None = get_cv2_rotation(config.rotation) self.rotation: int | None = get_cv2_rotation(config.rotation)
if self.height and self.width: if self.height and self.width:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]: if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.prerotated_width, self.prerotated_height = self.height, self.width self.capture_width, self.capture_height = self.height, self.width
else: else:
self.prerotated_width, self.prerotated_height = self.width, self.height self.capture_width, self.capture_height = self.width, self.height
def __str__(self) -> str: def __str__(self) -> str:
"""Returns a string representation of the camera instance."""
return f"{self.__class__.__name__}({self.serial_number})" return f"{self.__class__.__name__}({self.serial_number})"
@property @property
@@ -163,36 +153,69 @@ class RealSenseCamera(Camera):
"""Checks if the camera pipeline is started and streams are active.""" """Checks if the camera pipeline is started and streams are active."""
return self.rs_pipeline is not None and self.rs_profile is not None return self.rs_pipeline is not None and self.rs_profile is not None
def connect(self, warmup: bool = True):
"""
Connects to the RealSense camera specified in the configuration.
Initializes the RealSense pipeline, configures the required streams (color
and optionally depth), starts the pipeline, and validates the actual stream settings.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
ConnectionError: If the camera is found but fails to start the pipeline or no RealSense devices are detected at all.
RuntimeError: If the pipeline starts but fails to apply requested settings.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
self.rs_pipeline = rs.pipeline()
rs_config = self._make_rs_pipeline_config()
try:
self.rs_profile = self.rs_pipeline.start(rs_config)
logger.debug(f"Successfully started pipeline for camera {self.serial_number}.")
except RuntimeError as e:
self.rs_profile = None
self.rs_pipeline = None
raise ConnectionError(
f"Failed to open {self} camera. Run 'python -m lerobot.find_cameras realsense' for details about the available cameras in your system."
) from e
logger.debug(f"Validating stream configuration for {self}...")
self._validate_capture_settings()
if warmup:
if self.warmup_time is None:
raise ValueError(
f"Warmup time is not set for {self}. Please set a warmup time in the configuration."
)
logger.debug(f"Reading a warm-up frames for {self} for {self.warmup_time} seconds...")
start_time = time.time()
while time.time() - start_time < self.warmup_time:
self.read()
time.sleep(0.1)
logger.info(f"{self} connected.")
@staticmethod @staticmethod
def find_cameras(raise_when_empty: bool = True) -> List[Dict[str, Any]]: def find_cameras() -> List[Dict[str, Any]]:
""" """
Detects available Intel RealSense cameras connected to the system. Detects available Intel RealSense cameras connected to the system.
Args:
raise_when_empty (bool): If True, raises an OSError if no cameras are found.
Returns: Returns:
List[Dict[str, Any]]: A list of dictionaries, List[Dict[str, Any]]: A list of dictionaries,
where each dictionary contains 'type', 'id' (serial number), 'name', where each dictionary contains 'type', 'id' (serial number), 'name',
firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format). firmware version, USB type, and other available specs, and the default profile properties (width, height, fps, format).
Raises: Raises:
OSError: If `raise_when_empty` is True and no cameras are detected, OSError: If pyrealsense2 is not installed.
or if pyrealsense2 is not installed.
ImportError: If pyrealsense2 is not installed. ImportError: If pyrealsense2 is not installed.
""" """
found_cameras_info = [] found_cameras_info = []
context = rs.context() context = rs.context()
devices = context.query_devices() devices = context.query_devices()
if not devices:
logger.warning("No RealSense devices detected.")
if raise_when_empty:
raise OSError(
"No RealSense devices detected. Ensure cameras are connected, "
"library (`pyrealsense2`) is installed, and firmware is up-to-date."
)
for device in devices: for device in devices:
camera_info = { camera_info = {
"name": device.get_info(rs.camera_info.name), "name": device.get_info(rs.camera_info.name),
@@ -223,14 +246,13 @@ class RealSenseCamera(Camera):
camera_info["default_stream_profile"] = stream_info camera_info["default_stream_profile"] = stream_info
found_cameras_info.append(camera_info) found_cameras_info.append(camera_info)
logger.debug(f"Found RealSense camera: {camera_info}")
logger.info(f"Detected RealSense cameras: {[cam['id'] for cam in found_cameras_info]}") logger.info(f"Detected RealSense cameras: {[cam['id'] for cam in found_cameras_info]}")
return found_cameras_info return found_cameras_info
def _find_serial_number_from_name(self, name: str) -> str: def _find_serial_number_from_name(self, name: str) -> str:
"""Finds the serial number for a given unique camera name.""" """Finds the serial number for a given unique camera name."""
camera_infos = self.find_cameras(raise_when_empty=True) camera_infos = self.find_cameras()
found_devices = [cam for cam in camera_infos if str(cam["name"]) == name] found_devices = [cam for cam in camera_infos if str(cam["name"]) == name]
if not found_devices: if not found_devices:
@@ -250,24 +272,24 @@ class RealSenseCamera(Camera):
logger.info(f"Found serial number '{serial_number}' for camera name '{name}'.") logger.info(f"Found serial number '{serial_number}' for camera name '{name}'.")
return serial_number return serial_number
def _configure_realsense_settings(self) -> rs.config: def _make_rs_pipeline_config(self) -> rs.config:
"""Creates and configures the RealSense pipeline configuration object.""" """Creates and configures the RealSense pipeline configuration object."""
rs_config = rs.config() rs_config = rs.config()
rs.config.enable_device(rs_config, self.serial_number) rs.config.enable_device(rs_config, self.serial_number)
if self.width and self.height and self.fps: if self.width and self.height and self.fps:
logger.debug( logger.debug(
f"Requesting Color Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.rgb8}" f"Requesting Color Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.rgb8}"
) )
rs_config.enable_stream( rs_config.enable_stream(
rs.stream.color, self.prerotated_width, self.prerotated_height, rs.format.rgb8, self.fps rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps
) )
if self.use_depth: if self.use_depth:
logger.debug( logger.debug(
f"Requesting Depth Stream: {self.prerotated_width}x{self.prerotated_height} @ {self.fps} FPS, Format: {rs.format.z16}" f"Requesting Depth Stream: {self.capture_width}x{self.capture_height} @ {self.fps} FPS, Format: {rs.format.z16}"
) )
rs_config.enable_stream( rs_config.enable_stream(
rs.stream.depth, self.prerotated_width, self.prerotated_height, rs.format.z16, self.fps rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps
) )
else: else:
logger.debug(f"Requesting Color Stream: Default settings, Format: {rs.stream.color}") logger.debug(f"Requesting Color Stream: Default settings, Format: {rs.stream.color}")
@@ -295,111 +317,47 @@ class RealSenseCamera(Camera):
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.") raise DeviceNotConnectedError(f"Cannot validate settings for {self} as it is not connected.")
self._validate_fps(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile()) stream = self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile()
self._validate_width_and_height(self.rs_profile.get_stream(rs.stream.color).as_video_stream_profile())
if self.use_depth:
self._validate_fps(self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile())
self._validate_width_and_height(
self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()
)
def connect(self, do_warmup_read: bool = True):
"""
Connects to the RealSense camera specified in the configuration.
Initializes the RealSense pipeline, configures the required streams (color
and optionally depth), starts the pipeline, and validates the actual stream settings.
Raises:
DeviceAlreadyConnectedError: If the camera is already connected.
ValueError: If the configuration is invalid (e.g., missing serial/name, name not unique).
ConnectionError: If the camera is found but fails to start the pipeline.
RuntimeError: If the pipeline starts but fails to apply requested settings.
OSError: If no RealSense devices are detected at all.
"""
if self.is_connected:
raise DeviceAlreadyConnectedError(f"{self} is already connected.")
logger.debug(f"Attempting to connect to camera {self.serial_number}...")
self.rs_pipeline = rs.pipeline()
rs_config = self._configure_realsense_settings()
try:
self.rs_profile = self.rs_pipeline.start(rs_config)
logger.debug(f"Successfully started pipeline for camera {self.serial_number}.")
except RuntimeError as e:
self.rs_profile = None
self.rs_pipeline = None
raise ConnectionError(
f"Failed to open RealSense camera {self.serial_number}. Error: {e}. "
f"Run 'python -m find_cameras list-cameras' for details."
) from e
logger.debug(f"Validating stream configuration for {self.serial_number}...")
self._validate_capture_settings()
if do_warmup_read:
logger.debug(f"Reading a warm-up frame for {self.serial_number}...")
self.read() # NOTE(Steven): For now we just read one frame, we could also loop for X frames/secs
logger.info(f"Camera {self.serial_number} connected and configured successfully.")
def _validate_fps(self, stream) -> None:
"""Validates and sets the internal FPS based on actual stream FPS."""
actual_fps = stream.fps()
if self.fps is None: if self.fps is None:
self.fps = actual_fps self.fps = stream.fps()
logger.info(f"FPS not specified, using camera default: {self.fps} FPS.")
return
# Use math.isclose for robust float comparison if self.width is None or self.height is None:
if not math.isclose(self.fps, actual_fps, rel_tol=1e-3): actual_width = int(round(stream.width()))
logger.warning( actual_height = int(round(stream.height()))
f"Requested FPS {self.fps} for {self}, but camera reported {actual_fps}. " if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
"This might be due to camera limitations." self.width, self.height = actual_height, actual_width
) self.capture_width, self.capture_height = actual_width, actual_height
raise RuntimeError( else:
f"Failed to set requested FPS {self.fps} for {self}. Actual value reported: {actual_fps}." self.width, self.height = actual_width, actual_height
) self.capture_width, self.capture_height = actual_width, actual_height
logger.debug(f"FPS set to {actual_fps} for {self}.") logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
else:
self._validate_width_and_height(stream)
if self.use_depth:
stream = self.rs_profile.get_stream(rs.stream.depth).as_video_stream_profile()
self._validate_width_and_height(stream)
def _validate_width_and_height(self, stream) -> None: def _validate_width_and_height(self, stream) -> None:
"""Validates and sets the internal capture width and height based on actual stream width.""" """Validates and sets the internal capture width and height based on actual stream width."""
actual_width = int(round(stream.width())) actual_width = int(round(stream.width()))
actual_height = int(round(stream.height())) actual_height = int(round(stream.height()))
if self.width is None or self.height is None: if self.capture_width != actual_width:
if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE]:
self.width, self.height = actual_height, actual_width
self.prerotated_width, self.prerotated_height = actual_width, actual_height
else:
self.width, self.height = actual_width, actual_height
self.prerotated_width, self.prerotated_height = actual_width, actual_height
logger.info(f"Capture width set to camera default: {self.width}.")
logger.info(f"Capture height set to camera default: {self.height}.")
return
if self.prerotated_width != actual_width:
logger.warning(
f"Requested capture width {self.prerotated_width} for {self}, but camera reported {actual_width}."
)
raise RuntimeError( raise RuntimeError(
f"Failed to set requested capture width {self.prerotated_width} for {self}. Actual value: {actual_width}." f"Failed to set requested capture width {self.capture_width} for {self}. Actual value: {actual_width}."
) )
logger.debug(f"Capture width set to {actual_width} for {self}.") logger.debug(f"Capture width set to {actual_width} for {self}.")
if self.prerotated_height != actual_height: if self.capture_height != actual_height:
logger.warning(
f"Requested capture height {self.prerotated_height} for {self}, but camera reported {actual_height}."
)
raise RuntimeError( raise RuntimeError(
f"Failed to set requested capture height {self.prerotated_height} for {self}. Actual value: {actual_height}." f"Failed to set requested capture height {self.capture_height} for {self}. Actual value: {actual_height}."
) )
logger.debug(f"Capture height set to {actual_height} for {self}.") logger.debug(f"Capture height set to {actual_height} for {self}.")
def read_depth(self, timeout_ms: int = 5000) -> np.ndarray: def read_depth(self, timeout_ms: int = 100) -> np.ndarray:
""" """
Reads a single frame (depth) synchronously from the camera. Reads a single frame (depth) synchronously from the camera.
@@ -407,7 +365,7 @@ class RealSenseCamera(Camera):
from the camera hardware via the RealSense pipeline. from the camera hardware via the RealSense pipeline.
Args: Args:
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms. timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms.
Returns: Returns:
np.ndarray: The depth map as a NumPy array (height, width) np.ndarray: The depth map as a NumPy array (height, width)
@@ -420,35 +378,29 @@ class RealSenseCamera(Camera):
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.") raise DeviceNotConnectedError(f"{self} is not connected.")
if not self.use_depth: if not self.use_depth:
raise RuntimeError( raise RuntimeError(
f"Failed to capture depth frame from {self}. '.read_depth()'. Depth stream is not enabled." f"Failed to capture depth frame '.read_depth()'. Depth stream is not enabled for {self}."
) )
start_time = time.perf_counter() start_time = time.perf_counter()
ret, frame = self.rs_pipeline.try_wait_for_frames( ret, frame = self.rs_pipeline.try_wait_for_frames(timeout_ms=timeout_ms)
timeout_ms=timeout_ms
) # NOTE(Steven): This read has a timeout
if not ret or frame is None: if not ret or frame is None:
raise RuntimeError( raise RuntimeError(f"{self} failed to capture frame. Returned status='{ret}'.")
f"Failed to capture frame from {self}. '.read_depth()' returned status={ret} and frame is None."
)
depth_frame = frame.get_depth_frame() depth_frame = frame.get_depth_frame()
depth_map = np.asanyarray(depth_frame.get_data()) depth_map = np.asanyarray(depth_frame.get_data())
depth_map_processed = self._postprocess_image(depth_map) depth_map_processed = self._postprocess_image(depth_map, depth_frame=True)
read_duration_ms = (time.perf_counter() - start_time) * 1e3 read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return depth_map_processed return depth_map_processed
def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 5000) -> np.ndarray: def read(self, color_mode: ColorMode | None = None, timeout_ms: int = 100) -> np.ndarray:
""" """
Reads a single frame (color) synchronously from the camera. Reads a single frame (color) synchronously from the camera.
@@ -456,7 +408,7 @@ class RealSenseCamera(Camera):
from the camera hardware via the RealSense pipeline. from the camera hardware via the RealSense pipeline.
Args: Args:
timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 5000ms. timeout_ms (int): Maximum time in milliseconds to wait for a frame. Defaults to 100ms.
Returns: Returns:
np.ndarray: The captured color frame as a NumPy array np.ndarray: The captured color frame as a NumPy array
@@ -490,10 +442,11 @@ class RealSenseCamera(Camera):
read_duration_ms = (time.perf_counter() - start_time) * 1e3 read_duration_ms = (time.perf_counter() - start_time) * 1e3
logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms") logger.debug(f"{self} synchronous read took: {read_duration_ms:.1f}ms")
self.logs["timestamp_utc"] = capture_timestamp_utc()
return color_image_processed return color_image_processed
def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray: def _postprocess_image(
self, image: np.ndarray, color_mode: ColorMode | None = None, depth_frame: bool = False
) -> np.ndarray:
""" """
Applies color conversion, dimension validation, and rotation to a raw color frame. Applies color conversion, dimension validation, and rotation to a raw color frame.
@@ -516,15 +469,14 @@ class RealSenseCamera(Camera):
f"Invalid requested color mode '{color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}." f"Invalid requested color mode '{color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}."
) )
h, w, c = image.shape if depth_frame:
h, w = image.shape
else:
h, w, _c = image.shape
if h != self.prerotated_height or w != self.prerotated_width: if h != self.capture_height or w != self.capture_width:
raise RuntimeError( raise RuntimeError(
f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.prerotated_height}x{self.prerotated_width}) for {self}." f"Captured frame dimensions ({h}x{w}) do not match configured capture dimensions ({self.capture_height}x{self.capture_width}) for {self}."
)
if c != self.channels:
logger.warning(
f"Captured frame channels ({c}) do not match configured channels ({self.channels}) for {self}."
) )
processed_image = image processed_image = image
@@ -564,7 +516,7 @@ class RealSenseCamera(Camera):
logger.debug(f"Stopping read loop thread for {self}.") logger.debug(f"Stopping read loop thread for {self}.")
def _ensure_read_thread_running(self): def _start_read_thread(self) -> None:
"""Starts or restarts the background read thread if it's not running.""" """Starts or restarts the background read thread if it's not running."""
if self.thread is not None and self.thread.is_alive(): if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=0.1) self.thread.join(timeout=0.1)
@@ -572,15 +524,29 @@ class RealSenseCamera(Camera):
self.stop_event.set() self.stop_event.set()
self.stop_event = Event() self.stop_event = Event()
self.thread = Thread( self.thread = Thread(target=self._read_loop, args=(), name=f"{self}_read_loop")
target=self._read_loop, args=(), name=f"RealSenseReadLoop-{self}-{self.serial_number}"
)
self.thread.daemon = True self.thread.daemon = True
self.thread.start() self.thread.start()
logger.debug(f"Read thread started for {self}.") logger.debug(f"Read thread started for {self}.")
def _stop_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
logger.debug(f"Read thread stopped for {self}.")
# NOTE(Steven): Missing implementation for depth for now # NOTE(Steven): Missing implementation for depth for now
def async_read(self, timeout_ms: float = 2000) -> np.ndarray: def async_read(self, timeout_ms: float = 100) -> np.ndarray:
""" """
Reads the latest available frame data (color or color+depth) asynchronously. Reads the latest available frame data (color or color+depth) asynchronously.
@@ -591,7 +557,7 @@ class RealSenseCamera(Camera):
Args: Args:
timeout_ms (float): Maximum time in milliseconds to wait for a frame timeout_ms (float): Maximum time in milliseconds to wait for a frame
to become available in the queue. Defaults to 2000ms (2 seconds). to become available in the queue. Defaults to 100ms (0.1 seconds).
Returns: Returns:
np.ndarray: np.ndarray:
@@ -606,42 +572,18 @@ class RealSenseCamera(Camera):
raise DeviceNotConnectedError(f"{self} is not connected.") raise DeviceNotConnectedError(f"{self} is not connected.")
if self.thread is None or not self.thread.is_alive(): if self.thread is None or not self.thread.is_alive():
self._ensure_read_thread_running() self._start_read_thread()
try: try:
return self.frame_queue.get(timeout=timeout_ms / 1000.0) return self.frame_queue.get(timeout=timeout_ms / 1000.0)
except queue.Empty as e: except queue.Empty as e:
thread_alive = self.thread is not None and self.thread.is_alive() thread_alive = self.thread is not None and self.thread.is_alive()
logger.error(
f"Timeout waiting for frame from {self} queue after {timeout_ms}ms. "
f"(Read thread alive: {thread_alive})"
)
raise TimeoutError( raise TimeoutError(
f"Timed out waiting for frame from camera {self.serial_number} after {timeout_ms} ms. " f"Timed out waiting for frame from camera {self} after {timeout_ms} ms. "
f"Read thread alive: {thread_alive}." f"Read thread alive: {thread_alive}."
) from e ) from e
except Exception as e: except Exception as e:
logger.exception(f"Unexpected error getting frame data from queue for {self}: {e}") raise RuntimeError(f"Error getting frame data from queue for camera {self}: {e}") from e
raise RuntimeError(
f"Error getting frame data from queue for camera {self.serial_number}: {e}"
) from e
def _shutdown_read_thread(self):
"""Signals the background read thread to stop and waits for it to join."""
if self.stop_event is not None:
logger.debug(f"Signaling stop event for read thread of {self}.")
self.stop_event.set()
if self.thread is not None and self.thread.is_alive():
logger.debug(f"Waiting for read thread of {self} to join...")
self.thread.join(timeout=2.0)
if self.thread.is_alive():
logger.warning(f"Read thread for {self} did not terminate gracefully after 2 seconds.")
else:
logger.debug(f"Read thread for {self} joined successfully.")
self.thread = None
self.stop_event = None
def disconnect(self): def disconnect(self):
""" """
@@ -658,10 +600,8 @@ class RealSenseCamera(Camera):
f"Attempted to disconnect {self}, but it appears already disconnected." f"Attempted to disconnect {self}, but it appears already disconnected."
) )
logger.debug(f"Disconnecting from camera {self.serial_number}...")
if self.thread is not None: if self.thread is not None:
self._shutdown_read_thread() self._stop_read_thread()
if self.rs_pipeline is not None: if self.rs_pipeline is not None:
logger.debug(f"Stopping RealSense pipeline object for {self}.") logger.debug(f"Stopping RealSense pipeline object for {self}.")
@@ -669,4 +609,4 @@ class RealSenseCamera(Camera):
self.rs_pipeline = None self.rs_pipeline = None
self.rs_profile = None self.rs_profile = None
logger.info(f"Camera {self.serial_number} disconnected successfully.") logger.info(f"{self} disconnected.")

21
lerobot/common/cameras/intel/configuration_realsense.py → lerobot/common/cameras/realsense/configuration_realsense.py
View File

@@ -40,25 +40,20 @@ class RealSenseCameraConfig(CameraConfig):
fps: Requested frames per second for the color stream. fps: Requested frames per second for the color stream.
width: Requested frame width in pixels for the color stream. width: Requested frame width in pixels for the color stream.
height: Requested frame height in pixels for the color stream. height: Requested frame height in pixels for the color stream.
name: Optional human-readable name to identify the camera. serial_number_or_name: Unique serial number or human-readable name to identify the camera.
serial_number: Optional unique serial number to identify the camera.
Either name or serial_number must be provided.
color_mode: Color mode for image output (RGB or BGR). Defaults to RGB. color_mode: Color mode for image output (RGB or BGR). Defaults to RGB.
channels: Number of color channels (currently only 3 is supported).
use_depth: Whether to enable depth stream. Defaults to False. use_depth: Whether to enable depth stream. Defaults to False.
rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation. rotation: Image rotation setting (0°, 90°, 180°, or 270°). Defaults to no rotation.
Note: Note:
- Either name or serial_number must be specified, but not both. - Either name or serial_number must be specified.
- Depth stream configuration (if enabled) will use the same FPS as the color stream. - Depth stream configuration (if enabled) will use the same FPS as the color stream.
- The actual resolution and FPS may be adjusted by the camera to the nearest supported mode. - The actual resolution and FPS may be adjusted by the camera to the nearest supported mode.
- Only 3-channel color output (RGB/BGR) is currently supported. - For `fps`, `width` and `height`, either all of them need to be set, or none of them.
""" """
name: str | None = None serial_number_or_name: int | str
serial_number: int | None = None
color_mode: ColorMode = ColorMode.RGB color_mode: ColorMode = ColorMode.RGB
channels: int | None = 3
use_depth: bool = False use_depth: bool = False
rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION # NOTE(Steven): Check if draccus can parse to an enum rotation: Cv2Rotation = Cv2Rotation.NO_ROTATION # NOTE(Steven): Check if draccus can parse to an enum
@@ -78,10 +73,8 @@ class RealSenseCameraConfig(CameraConfig):
f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided." f"`rotation` is expected to be in {(Cv2Rotation.NO_ROTATION, Cv2Rotation.ROTATE_90, Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_270)}, but {self.rotation} is provided."
) )
if self.channels != 3: values = (self.fps, self.width, self.height)
raise NotImplementedError(f"Unsupported number of channels: {self.channels}") if any(v is not None for v in values) and any(v is None for v in values):
if bool(self.name) and bool(self.serial_number):
raise ValueError( raise ValueError(
f"One of them must be set: name or serial_number, but {self.name=} and {self.serial_number=} provided." "For `fps`, `width` and `height`, either all of them need to be set, or none of them."
) )

26
lerobot/common/cameras/utils.py
View File

@@ -37,7 +37,7 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s
cameras[key] = OpenCVCamera(cfg) cameras[key] = OpenCVCamera(cfg)
elif cfg.type == "intelrealsense": elif cfg.type == "intelrealsense":
from .intel.camera_realsense import RealSenseCamera from .realsense.camera_realsense import RealSenseCamera
cameras[key] = RealSenseCamera(cfg) cameras[key] = RealSenseCamera(cfg)
else: else:
@@ -46,24 +46,26 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s
return cameras return cameras
def get_cv2_rotation(rotation: Cv2Rotation) -> int: def get_cv2_rotation(rotation: Cv2Rotation) -> int | None:
import cv2 import cv2
return { if rotation == Cv2Rotation.ROTATE_90:
Cv2Rotation.ROTATE_270: cv2.ROTATE_90_COUNTERCLOCKWISE, return cv2.ROTATE_90_CLOCKWISE
Cv2Rotation.ROTATE_90: cv2.ROTATE_90_CLOCKWISE, elif rotation == Cv2Rotation.ROTATE_180:
Cv2Rotation.ROTATE_180: cv2.ROTATE_180, return cv2.ROTATE_180
}.get(rotation) elif rotation == Cv2Rotation.ROTATE_270:
return cv2.ROTATE_90_COUNTERCLOCKWISE
else:
return None
def get_cv2_backend() -> int: def get_cv2_backend() -> int:
import cv2 import cv2
return { if platform.system() == "Windows":
"Linux": cv2.CAP_DSHOW, return cv2.CAP_AVFOUNDATION
"Windows": cv2.CAP_AVFOUNDATION, else:
"Darwin": cv2.CAP_ANY, return cv2.CAP_ANY
}.get(platform.system(), cv2.CAP_V4L2)
def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path): def save_image(img_array: np.ndarray, camera_index: int, frame_index: int, images_dir: Path):

3
lerobot/common/envs/configs.py
View File

@@ -32,7 +32,8 @@ class EnvConfig(draccus.ChoiceRegistry, abc.ABC):
def type(self) -> str: def type(self) -> str:
return self.get_choice_name(self.__class__) return self.get_choice_name(self.__class__)
@abc.abstractproperty @property
@abc.abstractmethod
def gym_kwargs(self) -> dict: def gym_kwargs(self) -> dict:
raise NotImplementedError() raise NotImplementedError()

5
lerobot/common/motors/motors_bus.py
View File

@@ -749,7 +749,10 @@ class MotorsBus(abc.ABC):
# Move cursor up to overwrite the previous output # Move cursor up to overwrite the previous output
move_cursor_up(len(motors) + 3) move_cursor_up(len(motors) + 3)
# TODO(Steven, aliberts): add check to ensure mins and maxes are different same_min_max = [motor for motor in motors if mins[motor] == maxes[motor]]
if same_min_max:
raise ValueError(f"Some motors have the same min and max values:\n{pformat(same_min_max)}")
return mins, maxes return mins, maxes
def _normalize(self, data_name: str, ids_values: dict[int, int]) -> dict[int, float]: def _normalize(self, data_name: str, ids_values: dict[int, int]) -> dict[int, float]:

16
lerobot/common/robots/config.py
View File

@@ -27,15 +27,13 @@ class RobotConfig(draccus.ChoiceRegistry, abc.ABC):
calibration_dir: Path | None = None calibration_dir: Path | None = None
def __post_init__(self): def __post_init__(self):
if hasattr(self, "cameras"): if hasattr(self, "cameras") and self.cameras:
cameras = self.cameras for _, config in self.cameras.items():
if cameras: for attr in ["width", "height", "fps"]:
for cam_name, cam_config in cameras.items(): if getattr(config, attr) is None:
for attr in ["width", "height", "fps"]: raise ValueError(
if getattr(cam_config, attr) is None: f"Specifying '{attr}' is required for the camera to be used in a robot"
raise ValueError( )
f"Camera config for '{cam_name}' has None value for required attribute '{attr}'"
)
@property @property
def type(self) -> str: def type(self) -> str:

7
lerobot/common/robots/lekiwi/lekiwi.py
View File

@@ -16,6 +16,7 @@
import logging import logging
import time import time
from itertools import chain
from typing import Any from typing import Any
from lerobot.common.cameras.utils import make_cameras_from_configs from lerobot.common.cameras.utils import make_cameras_from_configs
@@ -183,6 +184,12 @@ class LeKiwi(Robot):
self.bus.enable_torque() self.bus.enable_torque()
def setup_motors(self) -> None:
for motor in chain(reversed(self.arm_motors), reversed(self.base_motors)):
input(f"Connect the controller board to the '{motor}' motor only and press enter.")
self.bus.setup_motor(motor)
print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
def get_observation(self) -> dict[str, Any]: def get_observation(self) -> dict[str, Any]:
if not self.is_connected: if not self.is_connected:
raise DeviceNotConnectedError(f"{self} is not connected.") raise DeviceNotConnectedError(f"{self} is not connected.")

12
lerobot/common/robots/robot.py
View File

@@ -49,15 +49,18 @@ class Robot(abc.ABC):
return f"{self.id} {self.__class__.__name__}" return f"{self.id} {self.__class__.__name__}"
# TODO(aliberts): create a proper Feature class for this that links with datasets # TODO(aliberts): create a proper Feature class for this that links with datasets
@abc.abstractproperty @property
@abc.abstractmethod
def observation_features(self) -> dict: def observation_features(self) -> dict:
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def action_features(self) -> dict: def action_features(self) -> dict:
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def is_connected(self) -> bool: def is_connected(self) -> bool:
pass pass
@@ -66,7 +69,8 @@ class Robot(abc.ABC):
"""Connects to the robot.""" """Connects to the robot."""
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
pass pass

2
lerobot/common/robots/stretch3/configuration_stretch3.py
View File

@@ -15,8 +15,8 @@
from dataclasses import dataclass, field from dataclasses import dataclass, field
from lerobot.common.cameras import CameraConfig from lerobot.common.cameras import CameraConfig
from lerobot.common.cameras.intel import RealSenseCameraConfig
from lerobot.common.cameras.opencv import OpenCVCameraConfig from lerobot.common.cameras.opencv import OpenCVCameraConfig
from lerobot.common.cameras.realsense import RealSenseCameraConfig
from ..config import RobotConfig from ..config import RobotConfig

12
lerobot/common/teleoperators/teleoperator.py
View File

@@ -47,15 +47,18 @@ class Teleoperator(abc.ABC):
def __str__(self) -> str: def __str__(self) -> str:
return f"{self.id} {self.__class__.__name__}" return f"{self.id} {self.__class__.__name__}"
@abc.abstractproperty @property
@abc.abstractmethod
def action_features(self) -> dict: def action_features(self) -> dict:
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def feedback_features(self) -> dict: def feedback_features(self) -> dict:
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def is_connected(self) -> bool: def is_connected(self) -> bool:
pass pass
@@ -64,7 +67,8 @@ class Teleoperator(abc.ABC):
"""Connects to the teleoperator.""" """Connects to the teleoperator."""
pass pass
@abc.abstractproperty @property
@abc.abstractmethod
def is_calibrated(self) -> bool: def is_calibrated(self) -> bool:
pass pass

1
lerobot/common/utils/control_utils.py
View File

@@ -38,7 +38,6 @@ from lerobot.common.utils.robot_utils import busy_wait
from lerobot.common.utils.utils import get_safe_torch_device, has_method from lerobot.common.utils.utils import get_safe_torch_device, has_method
# NOTE(Steven): Consider integrating this in camera class
def log_control_info(robot: Robot, dt_s, episode_index=None, frame_index=None, fps=None): def log_control_info(robot: Robot, dt_s, episode_index=None, frame_index=None, fps=None):
log_items = [] log_items = []
if episode_index is not None: if episode_index is not None:

9
lerobot/configs/policies.py
View File

@@ -78,15 +78,18 @@ class PreTrainedConfig(draccus.ChoiceRegistry, HubMixin, abc.ABC):
def type(self) -> str: def type(self) -> str:
return self.get_choice_name(self.__class__) return self.get_choice_name(self.__class__)
@abc.abstractproperty @property
@abc.abstractmethod
def observation_delta_indices(self) -> list | None: def observation_delta_indices(self) -> list | None:
raise NotImplementedError raise NotImplementedError
@abc.abstractproperty @property
@abc.abstractmethod
def action_delta_indices(self) -> list | None: def action_delta_indices(self) -> list | None:
raise NotImplementedError raise NotImplementedError
@abc.abstractproperty @property
@abc.abstractmethod
def reward_delta_indices(self) -> list | None: def reward_delta_indices(self) -> list | None:
raise NotImplementedError raise NotImplementedError

63
lerobot/find_cameras.py
View File

@@ -14,6 +14,16 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
"""
Helper to find the camera devices available in your system.
Example:
```shell
python -m lerobot.find_cameras
```
"""
import argparse import argparse
import concurrent.futures import concurrent.futures
import logging import logging
@@ -26,13 +36,12 @@ import numpy as np
from PIL import Image from PIL import Image
from lerobot.common.cameras.configs import ColorMode from lerobot.common.cameras.configs import ColorMode
from lerobot.common.cameras.intel.camera_realsense import RealSenseCamera
from lerobot.common.cameras.intel.configuration_realsense import RealSenseCameraConfig
from lerobot.common.cameras.opencv.camera_opencv import OpenCVCamera from lerobot.common.cameras.opencv.camera_opencv import OpenCVCamera
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig
from lerobot.common.cameras.realsense.camera_realsense import RealSenseCamera
from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig
logger = logging.getLogger(__name__) logger = logging.getLogger(__name__)
# logging.basicConfig(level=logging.INFO, format="%(asctime)s - %(levelname)s - %(module)s - %(message)s")
def find_all_opencv_cameras() -> List[Dict[str, Any]]: def find_all_opencv_cameras() -> List[Dict[str, Any]]:
@@ -45,7 +54,7 @@ def find_all_opencv_cameras() -> List[Dict[str, Any]]:
all_opencv_cameras_info: List[Dict[str, Any]] = [] all_opencv_cameras_info: List[Dict[str, Any]] = []
logger.info("Searching for OpenCV cameras...") logger.info("Searching for OpenCV cameras...")
try: try:
opencv_cameras = OpenCVCamera.find_cameras(raise_when_empty=False) opencv_cameras = OpenCVCamera.find_cameras()
for cam_info in opencv_cameras: for cam_info in opencv_cameras:
all_opencv_cameras_info.append(cam_info) all_opencv_cameras_info.append(cam_info)
logger.info(f"Found {len(opencv_cameras)} OpenCV cameras.") logger.info(f"Found {len(opencv_cameras)} OpenCV cameras.")
@@ -65,7 +74,7 @@ def find_all_realsense_cameras() -> List[Dict[str, Any]]:
all_realsense_cameras_info: List[Dict[str, Any]] = [] all_realsense_cameras_info: List[Dict[str, Any]] = []
logger.info("Searching for RealSense cameras...") logger.info("Searching for RealSense cameras...")
try: try:
realsense_cameras = RealSenseCamera.find_cameras(raise_when_empty=False) realsense_cameras = RealSenseCamera.find_cameras()
for cam_info in realsense_cameras: for cam_info in realsense_cameras:
all_realsense_cameras_info.append(cam_info) all_realsense_cameras_info.append(cam_info)
logger.info(f"Found {len(realsense_cameras)} RealSense cameras.") logger.info(f"Found {len(realsense_cameras)} RealSense cameras.")
@@ -170,7 +179,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None:
instance = OpenCVCamera(cv_config) instance = OpenCVCamera(cv_config)
elif cam_type == "RealSense": elif cam_type == "RealSense":
rs_config = RealSenseCameraConfig( rs_config = RealSenseCameraConfig(
serial_number=str(cam_id), serial_number_or_name=int(cam_id),
color_mode=ColorMode.RGB, color_mode=ColorMode.RGB,
) )
instance = RealSenseCamera(rs_config) instance = RealSenseCamera(rs_config)
@@ -180,7 +189,7 @@ def create_camera_instance(cam_meta: Dict[str, Any]) -> Dict[str, Any] | None:
if instance: if instance:
logger.info(f"Connecting to {cam_type} camera: {cam_id}...") logger.info(f"Connecting to {cam_type} camera: {cam_id}...")
instance.connect(do_warmup_read=False) instance.connect(warmup=False)
return {"instance": instance, "meta": cam_meta} return {"instance": instance, "meta": cam_meta}
except Exception as e: except Exception as e:
logger.error(f"Failed to connect or configure {cam_type} camera {cam_id}: {e}") logger.error(f"Failed to connect or configure {cam_type} camera {cam_id}: {e}")
@@ -293,28 +302,8 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser( parser = argparse.ArgumentParser(
description="Unified camera utility script for listing cameras and capturing images." description="Unified camera utility script for listing cameras and capturing images."
) )
subparsers = parser.add_subparsers(dest="command", help="Available commands")
# List cameras command parser.add_argument(
list_parser = subparsers.add_parser(
"list-cameras", help="Shows connected cameras. Optionally filter by type (realsense or opencv)."
)
list_parser.add_argument(
"camera_type",
type=str,
nargs="?",
default=None,
choices=["realsense", "opencv"],
help="Specify camera type to list (e.g., 'realsense', 'opencv'). Lists all if omitted.",
)
list_parser.set_defaults(func=lambda args: find_and_print_cameras(args.camera_type))
# Capture images command
capture_parser = subparsers.add_parser(
"capture-images",
help="Saves images from detected cameras (optionally filtered by type) using their default stream profiles.",
)
capture_parser.add_argument(
"camera_type", "camera_type",
type=str, type=str,
nargs="?", nargs="?",
@@ -322,19 +311,19 @@ if __name__ == "__main__":
choices=["realsense", "opencv"], choices=["realsense", "opencv"],
help="Specify camera type to capture from (e.g., 'realsense', 'opencv'). Captures from all if omitted.", help="Specify camera type to capture from (e.g., 'realsense', 'opencv'). Captures from all if omitted.",
) )
capture_parser.add_argument( parser.add_argument(
"--output-dir", "--output-dir",
type=Path, type=Path,
default="outputs/captured_images", default="outputs/captured_images",
help="Directory to save images. Default: outputs/captured_images", help="Directory to save images. Default: outputs/captured_images",
) )
capture_parser.add_argument( parser.add_argument(
"--record-time-s", "--record-time-s",
type=float, type=float,
default=6.0, default=6.0,
help="Time duration to attempt capturing frames. Default: 6 seconds.", help="Time duration to attempt capturing frames. Default: 6 seconds.",
) )
capture_parser.set_defaults( parser.set_defaults(
func=lambda args: save_images_from_all_cameras( func=lambda args: save_images_from_all_cameras(
output_dir=args.output_dir, output_dir=args.output_dir,
record_time_s=args.record_time_s, record_time_s=args.record_time_s,
@@ -344,14 +333,4 @@ if __name__ == "__main__":
args = parser.parse_args() args = parser.parse_args()
if args.command is None: args.func(args)
default_output_dir = capture_parser.get_default("output_dir")
default_record_time_s = capture_parser.get_default("record_time_s")
save_images_from_all_cameras(
output_dir=default_output_dir,
record_time_s=default_record_time_s,
camera_type_filter=None,
)
else:
args.func(args)

2
lerobot/record.py
View File

@@ -45,6 +45,8 @@ import rerun as rr
from lerobot.common.cameras import ( # noqa: F401 from lerobot.common.cameras import ( # noqa: F401
CameraConfig, # noqa: F401 CameraConfig, # noqa: F401
) )
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401
from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
from lerobot.common.datasets.image_writer import safe_stop_image_writer from lerobot.common.datasets.image_writer import safe_stop_image_writer
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset from lerobot.common.datasets.lerobot_dataset import LeRobotDataset
from lerobot.common.datasets.utils import build_dataset_frame, hw_to_dataset_features from lerobot.common.datasets.utils import build_dataset_frame, hw_to_dataset_features

2
lerobot/teleoperate.py
View File

@@ -38,6 +38,8 @@ import draccus
import numpy as np import numpy as np
import rerun as rr import rerun as rr
from lerobot.common.cameras.opencv.configuration_opencv import OpenCVCameraConfig # noqa: F401
from lerobot.common.cameras.realsense.configuration_realsense import RealSenseCameraConfig # noqa: F401
from lerobot.common.robots import ( # noqa: F401 from lerobot.common.robots import ( # noqa: F401
Robot, Robot,
RobotConfig, RobotConfig,

2
pyproject.toml
View File

@@ -86,7 +86,7 @@ dynamixel = ["dynamixel-sdk>=3.7.31"]
feetech = ["feetech-servo-sdk>=1.0.0"] feetech = ["feetech-servo-sdk>=1.0.0"]
intelrealsense = [ intelrealsense = [
"pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'", "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'",
"pyrealsense2-macosx>=2.54 ; sys_platform == 'darwin'", #NOTE(Steven): Check previous version for sudo issue "pyrealsense2-macosx>=2.54 ; sys_platform == 'darwin'",
] ]
pi0 = ["transformers>=4.48.0"] pi0 = ["transformers>=4.48.0"]
pusht = ["gym-pusht>=0.1.5 ; python_version < '4.0'"] pusht = ["gym-pusht>=0.1.5 ; python_version < '4.0'"]

0
tests/artifacts/cameras/fakecam_square_128x128.png → tests/artifacts/cameras/image_128x128.png
View File

0
tests/artifacts/cameras/fakecam_sd_160x120.png → tests/artifacts/cameras/image_160x120.png
View File

0
tests/artifacts/cameras/fakecam_hd_320x180.png → tests/artifacts/cameras/image_320x180.png
View File

0
tests/artifacts/cameras/fakecam_fullhd_480x270.png → tests/artifacts/cameras/image_480x270.png
View File

52
tests/cameras/test_opencv.py
View File

@@ -19,7 +19,7 @@
# pytest tests/cameras/test_opencv.py::test_connect # pytest tests/cameras/test_opencv.py::test_connect
# ``` # ```
import os from pathlib import Path
import numpy as np import numpy as np
import pytest import pytest
@@ -29,17 +29,18 @@ from lerobot.common.cameras.opencv import OpenCVCamera, OpenCVCameraConfig
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
# NOTE(Steven): more tests + assertions? # NOTE(Steven): more tests + assertions?
TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras") TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras"
DEFAULT_PNG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png") DEFAULT_PNG_FILE_PATH = TEST_ARTIFACTS_DIR / "image_160x120.png"
TEST_IMAGE_PATHS = [ TEST_IMAGE_PATHS = [
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_sd_160x120.png"), TEST_ARTIFACTS_DIR / "image_160x120.png",
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_hd_320x180.png"), TEST_ARTIFACTS_DIR / "image_320x180.png",
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_fullhd_480x270.png"), TEST_ARTIFACTS_DIR / "image_480x270.png",
os.path.join(TEST_ARTIFACTS_DIR, "fakecam_square_128x128.png"), TEST_ARTIFACTS_DIR / "image_128x128.png",
] ]
def test_base_class_implementation(): def test_abc_implementation():
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
config = OpenCVCameraConfig(index_or_path=0) config = OpenCVCameraConfig(index_or_path=0)
_ = OpenCVCamera(config) _ = OpenCVCamera(config)
@@ -49,7 +50,7 @@ def test_connect():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
assert camera.is_connected assert camera.is_connected
@@ -57,10 +58,10 @@ def test_connect():
def test_connect_already_connected(): def test_connect_already_connected():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
with pytest.raises(DeviceAlreadyConnectedError): with pytest.raises(DeviceAlreadyConnectedError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
def test_connect_invalid_camera_path(): def test_connect_invalid_camera_path():
@@ -68,7 +69,7 @@ def test_connect_invalid_camera_path():
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
with pytest.raises(ConnectionError): with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
def test_invalid_width_connect(): def test_invalid_width_connect():
@@ -80,14 +81,14 @@ def test_invalid_width_connect():
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
with pytest.raises(RuntimeError): with pytest.raises(RuntimeError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) @pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
def test_read(index_or_path): def test_read(index_or_path):
config = OpenCVCameraConfig(index_or_path=index_or_path) config = OpenCVCameraConfig(index_or_path=index_or_path)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
img = camera.read() img = camera.read()
@@ -105,7 +106,7 @@ def test_read_before_connect():
def test_disconnect(): def test_disconnect():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
camera.disconnect() camera.disconnect()
@@ -120,11 +121,11 @@ def test_disconnect_before_connect():
_ = camera.disconnect() _ = camera.disconnect()
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) @pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
def test_async_read(index_or_path): def test_async_read(index_or_path):
config = OpenCVCameraConfig(index_or_path=index_or_path) config = OpenCVCameraConfig(index_or_path=index_or_path)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
try: try:
img = camera.async_read() img = camera.async_read()
@@ -140,11 +141,13 @@ def test_async_read(index_or_path):
def test_async_read_timeout(): def test_async_read_timeout():
config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
try: try:
with pytest.raises(TimeoutError): with pytest.raises(TimeoutError):
camera.async_read(timeout_ms=0) camera.async_read(
timeout_ms=0
) # NOTE(Steven): This is flaky as sdometimes we actually get a frame
finally: finally:
if camera.is_connected: if camera.is_connected:
camera.disconnect() camera.disconnect()
@@ -158,7 +161,7 @@ def test_async_read_before_connect():
_ = camera.async_read() _ = camera.async_read()
@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS) @pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=[str(path.name) for path in TEST_IMAGE_PATHS])
@pytest.mark.parametrize( @pytest.mark.parametrize(
"rotation", "rotation",
[ [
@@ -167,15 +170,16 @@ def test_async_read_before_connect():
Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_180,
Cv2Rotation.ROTATE_270, Cv2Rotation.ROTATE_270,
], ],
ids=["no_rot", "rot90", "rot180", "rot270"],
) )
def test_all_rotations(rotation, index_or_path): def test_rotation(rotation, index_or_path):
filename = os.path.basename(index_or_path) filename = Path(index_or_path).name
dimensions = filename.split("_")[-1].split(".")[0] # Assumes filenames format (_wxh.png) dimensions = filename.split("_")[-1].split(".")[0] # Assumes filenames format (_wxh.png)
original_width, original_height = map(int, dimensions.split("x")) original_width, original_height = map(int, dimensions.split("x"))
config = OpenCVCameraConfig(index_or_path=index_or_path, rotation=rotation) config = OpenCVCameraConfig(index_or_path=index_or_path, rotation=rotation)
camera = OpenCVCamera(config) camera = OpenCVCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
img = camera.read() img = camera.read()
assert isinstance(img, np.ndarray) assert isinstance(img, np.ndarray)

124
tests/cameras/test_realsense.py
View File

@@ -19,7 +19,7 @@
# pytest tests/cameras/test_opencv.py::test_connect # pytest tests/cameras/test_opencv.py::test_connect
# ``` # ```
import os from pathlib import Path
from unittest.mock import patch from unittest.mock import patch
import numpy as np import numpy as np
@@ -29,95 +29,103 @@ from lerobot.common.cameras.configs import Cv2Rotation
from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
try: try:
from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
except (ImportError, ModuleNotFoundError): except (ImportError, ModuleNotFoundError, NameError):
pytest.skip("pyrealsense2 not available", allow_module_level=True) pytest.skip("pyrealsense2 not available", allow_module_level=True)
TEST_ARTIFACTS_DIR = os.path.join(os.path.dirname(os.path.dirname(__file__)), "artifacts", "cameras") TEST_ARTIFACTS_DIR = Path(__file__).parent.parent / "artifacts" / "cameras"
BAG_FILE_PATH = os.path.join(TEST_ARTIFACTS_DIR, "test_rs.bag") BAG_FILE_PATH = TEST_ARTIFACTS_DIR / "test_rs.bag"
# NOTE(Steven): Missing tests for depth # NOTE(Steven): For some reason these tests take ~20sec in macOS but only ~2sec in Linux.
# NOTE(Steven): Takes 20sec, the patch being the biggest bottleneck
# NOTE(Steven): more tests + assertions?
if not os.path.exists(BAG_FILE_PATH):
print(f"Warning: Bag file not found at {BAG_FILE_PATH}. Some tests might fail or be skipped.")
def mock_rs_config_enable_device_from_file(rs_config_instance, sn): def mock_rs_config_enable_device_from_file(rs_config_instance, _sn):
if not os.path.exists(BAG_FILE_PATH): return rs_config_instance.enable_device_from_file(str(BAG_FILE_PATH), repeat_playback=True)
raise FileNotFoundError(f"Test bag file not found: {BAG_FILE_PATH}")
return rs_config_instance.enable_device_from_file(BAG_FILE_PATH, repeat_playback=True)
def mock_rs_config_enable_device_bad_file(rs_config_instance, sn): def mock_rs_config_enable_device_bad_file(rs_config_instance, _sn):
return rs_config_instance.enable_device_from_file("non_existent_file.bag", repeat_playback=True) return rs_config_instance.enable_device_from_file("non_existent_file.bag", repeat_playback=True)
def test_base_class_implementation(): @pytest.fixture(name="patch_realsense", autouse=True)
config = RealSenseCameraConfig(serial_number=42) def fixture_patch_realsense():
"""Automatically mock pyrealsense2.config.enable_device for all tests."""
with patch(
"pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file
) as mock:
yield mock
def test_abc_implementation():
"""Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
config = RealSenseCameraConfig(serial_number_or_name=42)
_ = RealSenseCamera(config) _ = RealSenseCamera(config)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_connect():
def test_connect(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42)
config = RealSenseCameraConfig(serial_number=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
assert camera.is_connected assert camera.is_connected
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_connect_already_connected():
def test_connect_already_connected(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42)
config = RealSenseCameraConfig(serial_number=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
with pytest.raises(DeviceAlreadyConnectedError): with pytest.raises(DeviceAlreadyConnectedError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_bad_file) def test_connect_invalid_camera_path(patch_realsense):
def test_connect_invalid_camera_path(mock_enable_device): patch_realsense.side_effect = mock_rs_config_enable_device_bad_file
config = RealSenseCameraConfig(serial_number=42) config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
with pytest.raises(ConnectionError): with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_invalid_width_connect():
def test_invalid_width_connect(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42, width=99999, height=480, fps=30)
config = RealSenseCameraConfig(serial_number=42, width=99999, height=480, fps=30)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
with pytest.raises(ConnectionError): with pytest.raises(ConnectionError):
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_read():
def test_read(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
img = camera.read() img = camera.read()
assert isinstance(img, np.ndarray) assert isinstance(img, np.ndarray)
def test_read_depth():
config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30, use_depth=True)
camera = RealSenseCamera(config)
camera.connect(warmup=False)
img = camera.read_depth(timeout_ms=1000) # NOTE(Steven): Reading depth takes longer
assert isinstance(img, np.ndarray)
def test_read_before_connect(): def test_read_before_connect():
config = RealSenseCameraConfig(serial_number=42) config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError): with pytest.raises(DeviceNotConnectedError):
_ = camera.read() _ = camera.read()
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_disconnect():
def test_disconnect(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42)
config = RealSenseCameraConfig(serial_number=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
camera.disconnect() camera.disconnect()
@@ -125,18 +133,17 @@ def test_disconnect(mock_enable_device):
def test_disconnect_before_connect(): def test_disconnect_before_connect():
config = RealSenseCameraConfig(serial_number=42) config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError): with pytest.raises(DeviceNotConnectedError):
camera.disconnect() camera.disconnect()
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_async_read():
def test_async_read(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
try: try:
img = camera.async_read() img = camera.async_read()
@@ -149,22 +156,23 @@ def test_async_read(mock_enable_device):
camera.disconnect() # To stop/join the thread. Otherwise get warnings when the test ends camera.disconnect() # To stop/join the thread. Otherwise get warnings when the test ends
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_async_read_timeout():
def test_async_read_timeout(mock_enable_device): config = RealSenseCameraConfig(serial_number_or_name=42, width=640, height=480, fps=30)
config = RealSenseCameraConfig(serial_number=42, width=640, height=480, fps=30)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
try: try:
with pytest.raises(TimeoutError): with pytest.raises(TimeoutError):
camera.async_read(timeout_ms=0) camera.async_read(
timeout_ms=0
) # NOTE(Steven): This is flaky as sdometimes we actually get a frame
finally: finally:
if camera.is_connected: if camera.is_connected:
camera.disconnect() camera.disconnect()
def test_async_read_before_connect(): def test_async_read_before_connect():
config = RealSenseCameraConfig(serial_number=42) config = RealSenseCameraConfig(serial_number_or_name=42)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
with pytest.raises(DeviceNotConnectedError): with pytest.raises(DeviceNotConnectedError):
@@ -179,12 +187,12 @@ def test_async_read_before_connect():
Cv2Rotation.ROTATE_180, Cv2Rotation.ROTATE_180,
Cv2Rotation.ROTATE_270, Cv2Rotation.ROTATE_270,
], ],
ids=["no_rot", "rot90", "rot180", "rot270"],
) )
@patch("pyrealsense2.config.enable_device", side_effect=mock_rs_config_enable_device_from_file) def test_rotation(rotation):
def test_all_rotations(mock_enable_device, rotation): config = RealSenseCameraConfig(serial_number_or_name=42, rotation=rotation)
config = RealSenseCameraConfig(serial_number=42, rotation=rotation)
camera = RealSenseCamera(config) camera = RealSenseCamera(config)
camera.connect(do_warmup_read=False) camera.connect(warmup=False)
img = camera.read() img = camera.read()
assert isinstance(img, np.ndarray) assert isinstance(img, np.ndarray)

137
tests/mocks/mock_motors_bus.py Normal file
View File

@@ -0,0 +1,137 @@
# ruff: noqa: N802
from lerobot.common.motors.motors_bus import (
Motor,
MotorsBus,
)
DUMMY_CTRL_TABLE_1 = {
"Firmware_Version": (0, 1),
"Model_Number": (1, 2),
"Present_Position": (3, 4),
"Goal_Position": (11, 2),
}
DUMMY_CTRL_TABLE_2 = {
"Model_Number": (0, 2),
"Firmware_Version": (2, 1),
"Present_Position": (3, 4),
"Present_Velocity": (7, 4),
"Goal_Position": (11, 4),
"Goal_Velocity": (15, 4),
"Lock": (19, 1),
}
DUMMY_MODEL_CTRL_TABLE = {
"model_1": DUMMY_CTRL_TABLE_1,
"model_2": DUMMY_CTRL_TABLE_2,
"model_3": DUMMY_CTRL_TABLE_2,
}
DUMMY_BAUDRATE_TABLE = {
0: 1_000_000,
1: 500_000,
2: 250_000,
}
DUMMY_MODEL_BAUDRATE_TABLE = {
"model_1": DUMMY_BAUDRATE_TABLE,
"model_2": DUMMY_BAUDRATE_TABLE,
"model_3": DUMMY_BAUDRATE_TABLE,
}
DUMMY_ENCODING_TABLE = {
"Present_Position": 8,
"Goal_Position": 10,
}
DUMMY_MODEL_ENCODING_TABLE = {
"model_1": DUMMY_ENCODING_TABLE,
"model_2": DUMMY_ENCODING_TABLE,
"model_3": DUMMY_ENCODING_TABLE,
}
DUMMY_MODEL_NUMBER_TABLE = {
"model_1": 1234,
"model_2": 5678,
"model_3": 5799,
}
DUMMY_MODEL_RESOLUTION_TABLE = {
"model_1": 4096,
"model_2": 1024,
"model_3": 4096,
}
class MockPortHandler:
def __init__(self, port_name):
self.is_open: bool = False
self.baudrate: int
self.packet_start_time: float
self.packet_timeout: float
self.tx_time_per_byte: float
self.is_using: bool = False
self.port_name: str = port_name
self.ser = None
def openPort(self):
self.is_open = True
return self.is_open
def closePort(self):
self.is_open = False
def clearPort(self): ...
def setPortName(self, port_name):
self.port_name = port_name
def getPortName(self):
return self.port_name
def setBaudRate(self, baudrate):
self.baudrate: baudrate
def getBaudRate(self):
return self.baudrate
def getBytesAvailable(self): ...
def readPort(self, length): ...
def writePort(self, packet): ...
def setPacketTimeout(self, packet_length): ...
def setPacketTimeoutMillis(self, msec): ...
def isPacketTimeout(self): ...
def getCurrentTime(self): ...
def getTimeSinceStart(self): ...
def setupPort(self, cflag_baud): ...
def getCFlagBaud(self, baudrate): ...
class MockMotorsBus(MotorsBus):
available_baudrates = [500_000, 1_000_000]
default_timeout = 1000
model_baudrate_table = DUMMY_MODEL_BAUDRATE_TABLE
model_ctrl_table = DUMMY_MODEL_CTRL_TABLE
model_encoding_table = DUMMY_MODEL_ENCODING_TABLE
model_number_table = DUMMY_MODEL_NUMBER_TABLE
model_resolution_table = DUMMY_MODEL_RESOLUTION_TABLE
normalized_data = ["Present_Position", "Goal_Position"]
def __init__(self, port: str, motors: dict[str, Motor]):
super().__init__(port, motors)
self.port_handler = MockPortHandler(port)
def _assert_protocol_is_compatible(self, instruction_name): ...
def _handshake(self): ...
def _find_single_motor(self, motor, initial_baudrate): ...
def configure_motors(self): ...
def read_calibration(self): ...
def write_calibration(self, calibration_dict): ...
def disable_torque(self, motors, num_retry): ...
def _disable_torque(self, motor, model, num_retry): ...
def enable_torque(self, motors, num_retry): ...
def _get_half_turn_homings(self, positions): ...
def _encode_sign(self, data_name, ids_values): ...
def _decode_sign(self, data_name, ids_values): ...
def _split_into_byte_chunks(self, value, length): ...
def broadcast_ping(self, num_retry, raise_on_error): ...

140
tests/motors/test_motors_bus.py
View File

@@ -1,5 +1,3 @@
# ruff: noqa: N802
import re import re
from unittest.mock import patch from unittest.mock import patch
@@ -8,142 +6,16 @@ import pytest
from lerobot.common.motors.motors_bus import ( from lerobot.common.motors.motors_bus import (
Motor, Motor,
MotorNormMode, MotorNormMode,
MotorsBus,
assert_same_address, assert_same_address,
get_address, get_address,
get_ctrl_table, get_ctrl_table,
) )
from tests.mocks.mock_motors_bus import (
DUMMY_CTRL_TABLE_1 = { DUMMY_CTRL_TABLE_1,
"Firmware_Version": (0, 1), DUMMY_CTRL_TABLE_2,
"Model_Number": (1, 2), DUMMY_MODEL_CTRL_TABLE,
"Present_Position": (3, 4), MockMotorsBus,
"Goal_Position": (11, 2), )
}
DUMMY_CTRL_TABLE_2 = {
"Model_Number": (0, 2),
"Firmware_Version": (2, 1),
"Present_Position": (3, 4),
"Present_Velocity": (7, 4),
"Goal_Position": (11, 4),
"Goal_Velocity": (15, 4),
"Lock": (19, 1),
}
DUMMY_MODEL_CTRL_TABLE = {
"model_1": DUMMY_CTRL_TABLE_1,
"model_2": DUMMY_CTRL_TABLE_2,
"model_3": DUMMY_CTRL_TABLE_2,
}
DUMMY_BAUDRATE_TABLE = {
0: 1_000_000,
1: 500_000,
2: 250_000,
}
DUMMY_MODEL_BAUDRATE_TABLE = {
"model_1": DUMMY_BAUDRATE_TABLE,
"model_2": DUMMY_BAUDRATE_TABLE,
"model_3": DUMMY_BAUDRATE_TABLE,
}
DUMMY_ENCODING_TABLE = {
"Present_Position": 8,
"Goal_Position": 10,
}
DUMMY_MODEL_ENCODING_TABLE = {
"model_1": DUMMY_ENCODING_TABLE,
"model_2": DUMMY_ENCODING_TABLE,
"model_3": DUMMY_ENCODING_TABLE,
}
DUMMY_MODEL_NUMBER_TABLE = {
"model_1": 1234,
"model_2": 5678,
"model_3": 5799,
}
DUMMY_MODEL_RESOLUTION_TABLE = {
"model_1": 4096,
"model_2": 1024,
"model_3": 4096,
}
class MockPortHandler:
def __init__(self, port_name):
self.is_open: bool = False
self.baudrate: int
self.packet_start_time: float
self.packet_timeout: float
self.tx_time_per_byte: float
self.is_using: bool = False
self.port_name: str = port_name
self.ser = None
def openPort(self):
self.is_open = True
return self.is_open
def closePort(self):
self.is_open = False
def clearPort(self): ...
def setPortName(self, port_name):
self.port_name = port_name
def getPortName(self):
return self.port_name
def setBaudRate(self, baudrate):
self.baudrate: baudrate
def getBaudRate(self):
return self.baudrate
def getBytesAvailable(self): ...
def readPort(self, length): ...
def writePort(self, packet): ...
def setPacketTimeout(self, packet_length): ...
def setPacketTimeoutMillis(self, msec): ...
def isPacketTimeout(self): ...
def getCurrentTime(self): ...
def getTimeSinceStart(self): ...
def setupPort(self, cflag_baud): ...
def getCFlagBaud(self, baudrate): ...
class MockMotorsBus(MotorsBus):
available_baudrates = [500_000, 1_000_000]
default_timeout = 1000
model_baudrate_table = DUMMY_MODEL_BAUDRATE_TABLE
model_ctrl_table = DUMMY_MODEL_CTRL_TABLE
model_encoding_table = DUMMY_MODEL_ENCODING_TABLE
model_number_table = DUMMY_MODEL_NUMBER_TABLE
model_resolution_table = DUMMY_MODEL_RESOLUTION_TABLE
normalized_data = ["Present_Position", "Goal_Position"]
def __init__(self, port: str, motors: dict[str, Motor]):
super().__init__(port, motors)
self.port_handler = MockPortHandler(port)
def _assert_protocol_is_compatible(self, instruction_name): ...
def _handshake(self): ...
def _find_single_motor(self, motor, initial_baudrate): ...
def configure_motors(self): ...
def read_calibration(self): ...
def write_calibration(self, calibration_dict): ...
def disable_torque(self, motors, num_retry): ...
def _disable_torque(self, motor, model, num_retry): ...
def enable_torque(self, motors, num_retry): ...
def _get_half_turn_homings(self, positions): ...
def _encode_sign(self, data_name, ids_values): ...
def _decode_sign(self, data_name, ids_values): ...
def _split_into_byte_chunks(self, value, length): ...
def broadcast_ping(self, num_retry, raise_on_error): ...
@pytest.fixture @pytest.fixture

95
tests/robots/test_so100_follower.py Normal file
View File

@@ -0,0 +1,95 @@
from contextlib import contextmanager
from unittest.mock import MagicMock, patch
import pytest
from lerobot.common.robots.so100_follower import (
SO100Follower,
SO100FollowerConfig,
)
def _make_bus_mock() -> MagicMock:
"""Return a bus mock with just the attributes used by the robot."""
bus = MagicMock(name="FeetechBusMock")
bus.is_connected = False
def _connect():
bus.is_connected = True
def _disconnect(_disable=True):
bus.is_connected = False
bus.connect.side_effect = _connect
bus.disconnect.side_effect = _disconnect
@contextmanager
def _dummy_cm():
yield
bus.torque_disabled.side_effect = _dummy_cm
return bus
@pytest.fixture
def follower():
bus_mock = _make_bus_mock()
def _bus_side_effect(*_args, **kwargs):
bus_mock.motors = kwargs["motors"]
motors_order: list[str] = list(bus_mock.motors)
bus_mock.sync_read.return_value = {motor: idx for idx, motor in enumerate(motors_order, 1)}
bus_mock.sync_write.return_value = None
bus_mock.write.return_value = None
bus_mock.disable_torque.return_value = None
bus_mock.enable_torque.return_value = None
bus_mock.is_calibrated = True
return bus_mock
with (
patch(
"lerobot.common.robots.so100_follower.so100_follower.FeetechMotorsBus",
side_effect=_bus_side_effect,
),
patch.object(SO100Follower, "configure", lambda self: None),
):
cfg = SO100FollowerConfig(port="/dev/null")
robot = SO100Follower(cfg)
yield robot
if robot.is_connected:
robot.disconnect()
def test_connect_disconnect(follower):
assert not follower.is_connected
follower.connect()
assert follower.is_connected
follower.disconnect()
assert not follower.is_connected
def test_get_observation(follower):
follower.connect()
obs = follower.get_observation()
expected_keys = {f"{m}.pos" for m in follower.bus.motors}
assert set(obs.keys()) == expected_keys
for idx, motor in enumerate(follower.bus.motors, 1):
assert obs[f"{motor}.pos"] == idx
def test_send_action(follower):
follower.connect()
action = {f"{m}.pos": i * 10 for i, m in enumerate(follower.bus.motors, 1)}
returned = follower.send_action(action)
assert returned == action
goal_pos = {m: (i + 1) * 10 for i, m in enumerate(follower.bus.motors)}
follower.bus.sync_write.assert_called_once_with("Goal_Position", goal_pos)

2
tests/utils.py
View File

@@ -223,7 +223,7 @@ def make_camera(camera_type: str, **kwargs) -> Camera:
elif camera_type == "intelrealsense": elif camera_type == "intelrealsense":
serial_number = kwargs.pop("serial_number", INTELREALSENSE_SERIAL_NUMBER) serial_number = kwargs.pop("serial_number", INTELREALSENSE_SERIAL_NUMBER)
kwargs["serial_number"] = serial_number kwargs["serial_number"] = serial_number
from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig from lerobot.common.cameras.realsense import RealSenseCamera, RealSenseCameraConfig
config = RealSenseCameraConfig(**kwargs) config = RealSenseCameraConfig(**kwargs)
return RealSenseCamera(config) return RealSenseCamera(config)