diff --git a/lerobot/common/cameras/utils.py b/lerobot/common/cameras/utils.py index 56e71a48..f0b6ce5f 100644 --- a/lerobot/common/cameras/utils.py +++ b/lerobot/common/cameras/utils.py @@ -19,20 +19,3 @@ def make_cameras_from_configs(camera_configs: dict[str, CameraConfig]) -> dict[s raise ValueError(f"The motor type '{cfg.type}' is not valid.") return cameras - - -def make_camera(camera_type, **kwargs) -> Camera: - if camera_type == "opencv": - from .opencv import OpenCVCamera, OpenCVCameraConfig - - config = OpenCVCameraConfig(**kwargs) - return OpenCVCamera(config) - - elif camera_type == "intelrealsense": - from .intel import RealSenseCamera, RealSenseCameraConfig - - config = RealSenseCameraConfig(**kwargs) - return RealSenseCamera(config) - - else: - raise ValueError(f"The camera type '{camera_type}' is not valid.") diff --git a/lerobot/common/robots/__init__.py b/lerobot/common/robots/__init__.py new file mode 100644 index 00000000..58b868ed --- /dev/null +++ b/lerobot/common/robots/__init__.py @@ -0,0 +1,4 @@ +from .config import RobotConfig +from .robot import Robot + +__all__ = ["RobotConfig", "Robot"] diff --git a/lerobot/common/robots/utils.py b/lerobot/common/robots/utils.py index cb798632..8f253146 100644 --- a/lerobot/common/robots/utils.py +++ b/lerobot/common/robots/utils.py @@ -1,15 +1,9 @@ +import logging from typing import Protocol -from lerobot.common.robots.aloha.configuration_aloha import AlohaRobotConfig -from lerobot.common.robots.config_abc import ( - ManipulatorRobotConfig, - RobotConfig, -) -from lerobot.common.robots.koch.configuration_koch import KochBimanualRobotConfig, KochRobotConfig -from lerobot.common.robots.lekiwi.configuration_lekiwi import LeKiwiRobotConfig -from lerobot.common.robots.moss.configuration_moss import MossRobotConfig -from lerobot.common.robots.so_100.configuration_so_100 import So100RobotConfig -from lerobot.common.robots.stretch3.configuration_stretch3 import StretchRobotConfig +import numpy as np + +from lerobot.common.robots import RobotConfig def get_arm_id(name, arm_type): @@ -19,8 +13,8 @@ def get_arm_id(name, arm_type): return f"{name}_{arm_type}" +# TODO(aliberts): Remove and point to lerobot.common.robots.Robot class Robot(Protocol): - # TODO(rcadene, aliberts): Add unit test checking the protocol is implemented in the corresponding classes robot_type: str features: dict @@ -34,24 +28,39 @@ class Robot(Protocol): def make_robot_config(robot_type: str, **kwargs) -> RobotConfig: if robot_type == "aloha": + from .aloha.configuration_aloha import AlohaRobotConfig + return AlohaRobotConfig(**kwargs) elif robot_type == "koch": + from .koch.configuration_koch import KochRobotConfig + return KochRobotConfig(**kwargs) - elif robot_type == "koch_bimanual": - return KochBimanualRobotConfig(**kwargs) + # elif robot_type == "koch_bimanual": + # return KochBimanualRobotConfig(**kwargs) elif robot_type == "moss": + from .moss.configuration_moss import MossRobotConfig + return MossRobotConfig(**kwargs) elif robot_type == "so100": + from .so100.configuration_so100 import So100RobotConfig + return So100RobotConfig(**kwargs) elif robot_type == "stretch": + from .stretch3.configuration_stretch3 import StretchRobotConfig + return StretchRobotConfig(**kwargs) elif robot_type == "lekiwi": + from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig + return LeKiwiRobotConfig(**kwargs) else: raise ValueError(f"Robot type '{robot_type}' is not available.") def make_robot_from_config(config: RobotConfig): + from .lekiwi.configuration_lekiwi import LeKiwiRobotConfig + from .manipulator import ManipulatorRobotConfig + if isinstance(config, ManipulatorRobotConfig): from lerobot.common.robots.manipulator import ManipulatorRobot @@ -61,11 +70,31 @@ def make_robot_from_config(config: RobotConfig): return MobileManipulator(config) else: - from lerobot.common.robots.stretch3.robot_stretch3 import StretchRobot + from lerobot.common.robots.stretch3.robot_stretch3 import Stretch3Robot - return StretchRobot(config) + return Stretch3Robot(config) def make_robot(robot_type: str, **kwargs) -> Robot: config = make_robot_config(robot_type, **kwargs) return make_robot_from_config(config) + + +def ensure_safe_goal_position( + goal_pos: np.ndarray, present_pos: np.ndarray, max_relative_target: float | list[float] +): + # Cap relative action target magnitude for safety. + diff = goal_pos - present_pos + max_relative_target = np.array(max_relative_target) + safe_diff = np.min(diff, max_relative_target) + safe_diff = np.max(safe_diff, -max_relative_target) + safe_goal_pos = present_pos + safe_diff + + if not np.allclose(goal_pos, safe_goal_pos): + logging.warning( + "Relative goal position magnitude had to be clamped to be safe.\n" + f" requested relative goal position target: {diff}\n" + f" clamped relative goal position target: {safe_diff}" + ) + + return safe_goal_pos diff --git a/lerobot/configs/control.py b/lerobot/configs/control.py index 94922d1f..109b0ba9 100644 --- a/lerobot/configs/control.py +++ b/lerobot/configs/control.py @@ -4,7 +4,7 @@ from pathlib import Path import draccus -from lerobot.common.robots.config_abc import RobotConfig +from lerobot.common.robots import RobotConfig from lerobot.common.utils.utils import auto_select_torch_device, is_amp_available, is_torch_device_available from lerobot.configs import parser from lerobot.configs.policies import PreTrainedConfig diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index b6f58410..1a55c6fc 100644 --- a/lerobot/scripts/configure_motor.py +++ b/lerobot/scripts/configure_motor.py @@ -19,7 +19,7 @@ import time def get_motor_bus_cls(brand: str) -> tuple: if brand == "feetech": from lerobot.common.motors.configs import FeetechMotorsBusConfig - from lerobot.common.motors.feetech import ( + from lerobot.common.motors.feetech.feetech import ( MODEL_BAUDRATE_TABLE, SCS_SERIES_BAUDRATE_TABLE, FeetechMotorsBus, @@ -29,7 +29,7 @@ def get_motor_bus_cls(brand: str) -> tuple: elif brand == "dynamixel": from lerobot.common.motors.configs import DynamixelMotorsBusConfig - from lerobot.common.motors.dynamixel import ( + from lerobot.common.motors.dynamixel.dynamixel import ( MODEL_BAUDRATE_TABLE, X_SERIES_BAUDRATE_TABLE, DynamixelMotorsBus, diff --git a/tests/test_cameras.py b/tests/test_cameras.py index 686d1423..54f39df0 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -24,7 +24,7 @@ pytest -sx 'tests/test_cameras.py::test_camera[intelrealsense-True]' import numpy as np import pytest -from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from tests.utils import TEST_CAMERA_TYPES, make_camera, require_camera # Maximum absolute difference between two consecutive images recorded by a camera. @@ -57,11 +57,11 @@ def test_camera(request, camera_type, mock): camera = make_camera(**camera_kwargs) # Test reading, async reading, disconnecting before connecting raises an error - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): camera.read() - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): camera.async_read() - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): camera.disconnect() # Test deleting the object without connecting first @@ -76,7 +76,7 @@ def test_camera(request, camera_type, mock): assert camera.height is not None # Test connecting twice raises an error - with pytest.raises(RobotDeviceAlreadyConnectedError): + with pytest.raises(DeviceAlreadyConnectedError): camera.connect() # Test reading from the camera @@ -185,9 +185,9 @@ def test_camera(request, camera_type, mock): def test_save_images_from_cameras(tmp_path, request, camera_type, mock): # TODO(rcadene): refactor if camera_type == "opencv": - from lerobot.common.cameras.opencv import save_images_from_cameras + from lerobot.common.cameras.opencv.camera_opencv import save_images_from_cameras elif camera_type == "intelrealsense": - from lerobot.common.cameras.intelrealsense import save_images_from_cameras + from lerobot.common.cameras.intel.camera_realsense import save_images_from_cameras # Small `record_time_s` to speedup unit tests save_images_from_cameras(tmp_path, record_time_s=0.02, mock=mock) diff --git a/tests/test_motors.py b/tests/test_motors.py index 0e8ae9aa..0ad6b4d3 100644 --- a/tests/test_motors.py +++ b/tests/test_motors.py @@ -30,7 +30,7 @@ import time import numpy as np import pytest -from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.scripts.find_motors_bus_port import find_port from tests.utils import TEST_MOTOR_TYPES, make_motors_bus, require_motor @@ -89,11 +89,11 @@ def test_motors_bus(request, motor_type, mock): motors_bus = make_motors_bus(motor_type, mock=mock) # Test reading and writing before connecting raises an error - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): motors_bus.read("Torque_Enable") - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): motors_bus.write("Torque_Enable", 1) - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): motors_bus.disconnect() # Test deleting the object without connecting first @@ -104,7 +104,7 @@ def test_motors_bus(request, motor_type, mock): motors_bus.connect() # Test connecting twice raises an error - with pytest.raises(RobotDeviceAlreadyConnectedError): + with pytest.raises(DeviceAlreadyConnectedError): motors_bus.connect() # Test disabling torque and reading torque on all motors diff --git a/tests/test_robots.py b/tests/test_robots.py index 90fd8819..6def3a87 100644 --- a/tests/test_robots.py +++ b/tests/test_robots.py @@ -26,8 +26,8 @@ pytest -sx 'tests/test_robots.py::test_robot[aloha-True]' import pytest import torch +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError from lerobot.common.robots.utils import make_robot -from lerobot.common.utils.robot_utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError from tests.utils import TEST_ROBOT_TYPES, mock_calibration_dir, require_robot @@ -54,15 +54,15 @@ def test_robot(tmp_path, request, robot_type, mock): # Test using robot before connecting raises an error robot = make_robot(**robot_kwargs) - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): robot.teleop_step() - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): robot.teleop_step(record_data=True) - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): robot.capture_observation() - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): robot.send_action(None) - with pytest.raises(RobotDeviceNotConnectedError): + with pytest.raises(DeviceNotConnectedError): robot.disconnect() # Test deleting the object without connecting first @@ -74,7 +74,7 @@ def test_robot(tmp_path, request, robot_type, mock): assert robot.is_connected # Test connecting twice raises an error - with pytest.raises(RobotDeviceAlreadyConnectedError): + with pytest.raises(DeviceAlreadyConnectedError): robot.connect() # TODO(rcadene, aliberts): Test disconnecting with `__del__` instead of `disconnect` diff --git a/tests/utils.py b/tests/utils.py index 9ffa9ad2..12b30597 100644 --- a/tests/utils.py +++ b/tests/utils.py @@ -23,9 +23,8 @@ import pytest import torch from lerobot import available_cameras, available_motors, available_robots -from lerobot.common.cameras.utils import Camera -from lerobot.common.cameras.utils import make_camera as make_camera_device -from lerobot.common.motors.utils import MotorsBus +from lerobot.common.cameras import Camera +from lerobot.common.motors.motors_bus import MotorsBus from lerobot.common.motors.utils import make_motors_bus as make_motors_bus_device from lerobot.common.utils.import_utils import is_package_available @@ -306,11 +305,19 @@ def mock_calibration_dir(calibration_dir): def make_camera(camera_type: str, **kwargs) -> Camera: if camera_type == "opencv": camera_index = kwargs.pop("camera_index", OPENCV_CAMERA_INDEX) - return make_camera_device(camera_type, camera_index=camera_index, **kwargs) + kwargs["camera_index"] = camera_index + from lerobot.common.cameras.opencv import OpenCVCamera, OpenCVCameraConfig + + config = OpenCVCameraConfig(**kwargs) + return OpenCVCamera(config) elif camera_type == "intelrealsense": serial_number = kwargs.pop("serial_number", INTELREALSENSE_SERIAL_NUMBER) - return make_camera_device(camera_type, serial_number=serial_number, **kwargs) + kwargs["serial_number"] = serial_number + from lerobot.common.cameras.intel import RealSenseCamera, RealSenseCameraConfig + + config = RealSenseCameraConfig(**kwargs) + return RealSenseCamera(config) else: raise ValueError(f"The camera type '{camera_type}' is not valid.")